34 namespace atlascar_base
45 status_freq_(
"Gearbox",updater_,diagnostic_updater::FrequencyStatusParam(&status_max_frequency_,&status_min_frequency_, 0.1, 10)),
46 status_max_frequency_(8.0),
47 status_min_frequency_(12.0)
76 status_pub_ =
nh_.advertise<atlascar_base::GearboxStatus>(
"status", 1);
82 string received_message;
105 sprintf(send,
"sg %d",
command_->gear);
106 comm_->Send(send,strlen(send)+1);
107 comm_->perr(
"Failed to send");
116 comm_->Send(send,strlen(send)+1);
117 comm_->perr(
"Failed to send");
124 status_.header.stamp = ros::Time::now();
157 struct timeval timeout = {1,0};
158 setsockopt(
comm_->GetSocket(),SOL_SOCKET,SO_SNDTIMEO,&timeout,
sizeof(timeout));
159 setsockopt(
comm_->GetSocket(),SOL_SOCKET,SO_RCVTIMEO,&timeout,
sizeof(timeout));
164 std::cout<<
"Cannot connect to gearbox: "<<
comm_->err_msg<<
" "<<strerror(errno)<<std::endl;
170 std::cout<<
"reconnection sucessfull."<<std::endl;
186 sscanf(message.c_str(),
"%*c %s",opcode);
190 status_.status=
"command unknown";
194 sscanf(message.c_str(),
"%*c %*s %c",&code);
198 status_.status=
"command invalid";
204 sscanf(message.c_str(),
"%*c %*s %*c %d %*c %d",¤t_gear,&next_gear);
207 status_.status=
"changing to "+boost::lexical_cast<std::string>(next_gear);
212 sscanf(message.c_str(),
"%*c %*s %*c %d",¤t_gear);
220 status_.status=
"unrecognised status";
235 memset(recv_msg,0,
sizeof(recv_msg));
239 if(
comm_->Receive(rec_buf,1)!=0)
242 comm_->perr(
"Cannot read");
252 if(
comm_->Receive(rec_buf,1)!=0)
255 comm_->perr(
"Cannot read");
260 recv_msg[i++]=rec_buf[0];
ros::Subscriber command_sub_
Ros command subscriber.
atlascar_base::GearboxStatus status_
Status message.
Gearbox class declaration.
~Gearbox()
Class destructor.
Gearbox(const ros::NodeHandle &nh)
Class constructor.
diagnostic_updater::HeaderlessTopicDiagnostic status_freq_
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.
atlascar_base::GearboxCommandPtr command_
Command message pointer.
void commandCallback(const atlascar_base::GearboxCommandPtr &command)
Command message handler.
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.