34 namespace atlascar_base
37 Plc::Plc(
const ros::NodeHandle& nh,std::string ip, std::string port)
42 status_freq_(
"Plc",updater_,diagnostic_updater::FrequencyStatusParam(&status_max_frequency_,&status_min_frequency_, 0.1, 10)),
43 status_max_frequency_(10.0),
44 status_min_frequency_(20.0),
45 comm_(io_service_,server_ip_,server_port_,std::string(
"\x03"))
103 status_pub_ =
nh_.advertise<atlascar_base::PlcStatus>(
"status", 1);
109 boost::thread thread(boost::bind(&boost::asio::io_service::run, &
io_service_));
122 cout<<
"Error!! Invalid command in queue, safety message removed by user!!"<<endl;
129 }
catch(std::exception& e)
131 std::cout <<
"Exception: " << e.what() <<
"\n";
149 std::size_t found=data.find(
"\x02");
150 data.erase(0,found+1);
151 data.erase(data.end()-1,data.end());
154 double steering_wheel;
164 sscanf(data.c_str(),
"%*s %lf %*s %lf %*s %lf %*s %lf %*s %d %*s %d %*s %lf %*s %lf %*s %d %*s %d",&throttle,&steering_wheel,&brake,&clutch,&handbrake,&gear,&speed,&rpm,&emergency,&error);
189 status_.header.stamp = ros::Time::now();
197 char message_string[2000];
202 sprintf(message_string,
"STTP %.4E ",0.0);
204 sprintf(text,
"SSWP %.4E ",steering_wheel);
205 strcat(message_string,text);
207 sprintf(text,
"SBKP %.4E ",
command_->brake);
208 strcat(message_string,text);
210 sprintf(text,
"SCLP %.4E ",
command_->clutch);
211 strcat(message_string,text);
213 sprintf(text,
"SHBP %10d ",0);
214 strcat(message_string,text);
216 sprintf(text,
"SGER %10d ",0);
217 strcat(message_string,text);
219 sprintf(text,
"SIGN %10d ",
command_->ignition);
220 strcat(message_string,text);
222 sprintf(text,
"SVHS %.4E ",0.0);
223 strcat(message_string,text);
225 sprintf(text,
"SVBS %10d ",1);
226 strcat(message_string,text);
228 sprintf(text,
"SFAL %10d ",
command_->lights_warning);
229 strcat(message_string,text);
231 sprintf(text,
"SHIL %10d ",
command_->lights_high);
232 strcat(message_string,text);
234 sprintf(text,
"SHEL %10d ",
command_->lights_medium);
235 strcat(message_string,text);
237 sprintf(text,
"SRTL %10d ",
command_->lights_right);
238 strcat(message_string,text);
240 sprintf(text,
"SLTL %10d ",
command_->lights_left);
241 strcat(message_string,text);
243 sprintf(text,
"STTA %10d ",0);
244 strcat(message_string,text);
246 sprintf(text,
"SSWA %10d ",
command_->auto_direction);
247 strcat(message_string,text);
249 sprintf(text,
"SBKA %10d ",
command_->auto_brake);
250 strcat(message_string,text);
252 sprintf(text,
"SCLA %10d ",
command_->auto_clutch);
253 strcat(message_string,text);
255 sprintf(text,
"SHBA %10d ",0);
256 strcat(message_string,text);
258 sprintf(text,
"SGEA %10d ",0);
259 strcat(message_string,text);
261 sprintf(text,
"EEST %10d ",
command_->emergency);
262 strcat(message_string,text);
264 sprintf(text,
"SIGA %10d ",
command_->auto_ignition);
265 strcat(message_string,text);
274 char formatted_message[2000];
276 memset(formatted_message,0,
sizeof(formatted_message));
277 sprintf(formatted_message,
"%c%s%c",2,message_string,3);
279 std::string msg = std::string(formatted_message);
280 if(
verbose_)std::cout<<
">> "<<msg<<std::endl;
281 comm_.write(std::string(formatted_message));
TopicQueuePriority< atlascar_base::PlcCommandPtr > command_queue_
Command queue holding class.
boost::asio::io_service io_service_
Input/Output communication service.
atlascar_base::PlcStatus status_
Status message.
void commandCallback(const atlascar_base::PlcCommandPtr &command)
Command message handler.
bool verbose_
Verbose mode.
void setupMessaging()
Start ros message subscribing and advertising.
diagnostic_updater::Updater updater_
Diagnostics class.
void newData(string data)
This function will be called (asynchronously) upon arrival of new data.
SimpleCalibration brake_calibration_
Calibration of the brake pedal.
AsyncClient comm_
Asynchronous tcp/ip communication object.
void loop()
Start main control loop.
ros::Publisher status_pub_
Ros status publisher.
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
Diagnostics function handler.
int sendMessage(char *message_string)
Send a message.
SimpleCalibration steering_wheel_to_plc_calibration_
Calibration of the steering wheel to the plc.
SimpleCalibration clutch_calibration_
Calibration of the clutch pedal.
SimpleCalibration steering_wheel_calibration_
Calibration of the steering wheel from the plc.
Plc(const ros::NodeHandle &nh, std::string ip, std::string port)
Class constructor.
atlascar_base::PlcCommandPtr command_
Command message pointer.
atlascar_base::PlcCommandPtr safety_command_
Safety command message pointer.
double remap(double value)
Linear mapping function.
diagnostic_updater::HeaderlessTopicDiagnostic status_freq_
Frequency diagnostics tool.
ros::NodeHandle nh_
Ros node handler.
ros::Subscriber command_sub_
Ros command subscriber.
int sendCommand(void)
Send a global command down to the plc.
void init()
Initialize the class.