39 #include <tcp_client/AsyncClient.h>
41 #include <atlascar_base/PlcStatus.h>
42 #include <atlascar_base/PlcCommand.h>
44 #include <topic_priority/topic_priority.h>
46 #include <boost/format.hpp>
47 #include <boost/thread/thread.hpp>
48 #include <boost/spirit/include/qi.hpp>
49 #include <boost/spirit/include/phoenix_core.hpp>
50 #include <boost/spirit/include/phoenix_operator.hpp>
53 #include <diagnostic_updater/diagnostic_updater.h>
54 #include <diagnostic_updater/publisher.h>
58 namespace atlascar_base
80 assert(minimum_value_!=maximum_value_);
81 assert(minimum_required_!=maximum_required_);
83 if(minimum_value_>maximum_value_)
84 return remapInverted(value);
86 if(value<minimum_value_)
89 if(value>maximum_value_)
92 double m = (maximum_required_-minimum_required_)/(maximum_value_-minimum_value_);
93 double b = maximum_required_ - m*maximum_value_;
101 assert(minimum_value_!=maximum_value_);
102 assert(minimum_required_!=maximum_required_);
104 if(value>minimum_value_)
105 value=minimum_value_;
107 if(value<maximum_value_)
108 value=maximum_value_;
110 double m = (maximum_required_-minimum_required_)/(maximum_value_-minimum_value_);
111 double b = maximum_required_ - m*maximum_value_;
150 Plc(
const ros::NodeHandle& nh,std::string ip, std::string port);
171 void setupMessaging();
194 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
196 if(!comm_.isConnected())
198 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"No connection to hardware.");
199 stat.add(
"Status",comm_.error_.message());
200 stat.add(
"Trying to connect to",server_ip_);
201 stat.add(
"Port number",server_port_);
204 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
"No problems.");
205 stat.add(
"Connected with",server_ip_);
206 stat.add(
"Port number",server_port_);
208 boost::format fmt(
"%.2f");
209 fmt % (ros::Time::now()-status_.header.stamp).toSec();
211 stat.add(
"Last message sent", fmt.str()+
" seconds ago" );
221 void newData(
string data);
229 int sendCommand(
void);
237 int sendMessage(
char*message_string);
245 void commandCallback(
const atlascar_base::PlcCommandPtr& command);
281 char received_message_[1024];
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.
bool verbose_
Verbose mode.
diagnostic_updater::Updater updater_
Diagnostics class.
Plc communication and control class.
SimpleCalibration brake_calibration_
Calibration of the brake pedal.
AsyncClient comm_
Asynchronous tcp/ip communication object.
ros::Publisher status_pub_
Ros status publisher.
std::string server_ip_
Ip of the Plc server.
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
Diagnostics function handler.
double status_min_frequency_
Minimum admissible frequency.
SimpleCalibration steering_wheel_to_plc_calibration_
Calibration of the steering wheel to the plc.
double status_max_frequency_
Maximum admissible frequency.
SimpleCalibration clutch_calibration_
Calibration of the clutch pedal.
std::string server_port_
Port of the Plc server.
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.
double remapInverted(double value)
ros::NodeHandle nh_
Ros node handler.
int connection_status_
Current connection status.
ros::Subscriber command_sub_
Ros command subscriber.