37 #include <boost/lexical_cast.hpp>
38 #include <tcp_client/class_tcp.h>
40 #include <atlascar_base/GearboxStatus.h>
41 #include <atlascar_base/GearboxCommand.h>
43 #include <topic_priority/topic_priority.h>
45 #include <diagnostic_updater/diagnostic_updater.h>
46 #include <diagnostic_updater/publisher.h>
48 namespace atlascar_base
81 Gearbox(
const ros::NodeHandle& nh);
114 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
118 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"No connection to hardware.");
119 stat.add(
"Status",
"Check ethernet connection!");
120 stat.add(
"Trying to connect to",
ip_);
122 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
"No problems.");
ros::Subscriber command_sub_
Ros command subscriber.
atlascar_base::GearboxStatus status_
Status message.
~Gearbox()
Class destructor.
Gearbox(const ros::NodeHandle &nh)
Class constructor.
diagnostic_updater::HeaderlessTopicDiagnostic status_freq_
bool verbose_
Verbose mode.
int maintainConnection()
Keep connection alive.
tcp_client * comm_
TCP client communication class pointer.
int interpreterMessage(string &message)
TopicQueuePriority< atlascar_base::GearboxCommandPtr > command_queue_
Command queue holding class.
atlascar_base::GearboxCommandPtr safety_command_
Safety command message pointer.
void loop()
Start main control loop.
ros::Publisher status_pub_
Ros status publisher.
diagnostic_updater::Updater updater_
ros::NodeHandle nh_
Ros node handler.
double status_min_frequency_
atlascar_base::GearboxCommandPtr command_
Command message pointer.
void commandCallback(const atlascar_base::GearboxCommandPtr &command)
Command message handler.
double status_max_frequency_
This class is not fully documented!
int receiveMessage(string &message)
void setupMessaging()
Start ros message subscribing and advertising.
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
int connection_status_
Current connection status.
void init()
Initialize the class.