37 #include <boost/lexical_cast.hpp>
39 #include <tcp_client/AsyncClient.h>
41 #include <diagnostic_updater/diagnostic_updater.h>
42 #include <diagnostic_updater/publisher.h>
44 #include <atlascar_base/ThrottleStatus.h>
45 #include <atlascar_base/ThrottleCommand.h>
47 #include <topic_priority/topic_priority.h>
49 #include <boost/thread/thread.hpp>
50 #include <boost/format.hpp>
51 #include <boost/spirit/include/qi.hpp>
52 #include <boost/spirit/include/phoenix_core.hpp>
53 #include <boost/spirit/include/phoenix_operator.hpp>
57 namespace atlascar_base
91 Throttle(
const ros::NodeHandle& nh,std::string ip,std::string port):
93 current_mode_(MANUAL),
95 server_port_(port), status_freq_(
"Throttle",updater_,diagnostic_updater::FrequencyStatusParam(&status_min_frequency_,&status_max_frequency_, 0.01, 10)),
96 status_max_frequency_(51.0),
97 status_min_frequency_(49.0),
98 comm_(io_service_,server_ip_,server_port_)
117 safety_command_.reset(
new atlascar_base::ThrottleCommand);
120 safety_command_->lifetime=INFINITY;
121 safety_command_->priority=0;
122 safety_command_->throttle=0;
123 safety_command_->mode=MANUAL;
126 command_queue_.push_msg(safety_command_);
129 updater_.setHardwareID(
"throttle");
133 comm_.readHandler.connect(boost::bind(&Throttle::newData,
this,_1));
136 comm_.connectHandler.connect(boost::bind(&Throttle::connectHandler,
this));
148 command_sub_ = nh_.subscribe(
"command", 1, &Throttle::commandCallback,
this);
149 status_pub_ = nh_.advertise<atlascar_base::ThrottleStatus>(
"status", 1);
164 boost::thread thread(boost::bind(&boost::asio::io_service::run, &io_service_));
173 command_=command_queue_.top_msg();
176 if(status_.mode!=(
int)command_->mode)
178 boost::format fmt(
"set mode %d\n");
179 fmt % command_->mode;
183 comm_.write(fmt.str());
184 }
catch(std::exception& e)
186 std::cout<<
"Cannot set mode: "<<e.what()<<std::endl;
191 if(status_.mode==AUTO)
193 boost::format fmt(
"set throttle %lf\n");
194 fmt % command_->throttle;
198 comm_.write(fmt.str());
199 }
catch(std::exception& e)
201 std::cout<<
"Cannot set throttle: "<<e.what()<<std::endl;
205 if( comm_.isConnected() && (ros::Time::now()-status_.header.stamp).toSec() > 1.0)
226 enum {MANUAL=0,AUTO=1};
234 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
236 if(!comm_.isConnected())
238 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"No connection to hardware.");
239 stat.add(
"Status",comm_.error_.message());
240 stat.add(
"Trying to connect to",server_ip_);
241 stat.add(
"Port number",server_port_);
244 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
"No problems.");
245 stat.add(
"Connected with",server_ip_);
246 stat.add(
"Port number",server_port_);
248 boost::format fmt(
"%.4f");
249 fmt % (ros::Time::now()-status_.header.stamp).toSec();
251 stat.add(
"Last message received", fmt.str()+
" seconds ago" );
264 comm_.write(
"start");
265 }
catch(std::exception& e)
267 std::cout <<
"Exception: " << e.what() <<
"\n";
270 status_.header.stamp=ros::Time::now();
280 command_queue_.push_msg(command);
292 namespace qi = boost::spirit::qi;
295 using qi::ascii::space;
296 using boost::spirit::ascii::string;
298 using boost::phoenix::ref;
301 qi::phrase_parse(data.begin(),data.end(),
302 string(
"mode") >> int_[ref(status_.mode) = _1] >>
303 string(
"throttle") >> double_[ref(status_.throttle) = _1] >>
304 string(
"pedal") >> double_[ref(status_.footPedal) = _1]
307 status_.header.stamp = ros::Time::now();
308 status_.header.frame_id =
"";
309 status_pub_.publish(status_);
ros::Subscriber command_sub_
Ros command subscriber.
void newData(string data)
This function will be called (asynchronously) upon arrival of new data.
double status_min_frequency_
Minimum admissible frequency.
atlascar_base::ThrottleCommandPtr command_
Command message pointer.
diagnostic_updater::Updater updater_
Diagnostics class.
Throttle monitor and control class.
std::string server_port_
Throttle server port.
void setupMessaging()
Start ros message subscribing and advertising.
atlascar_base::ThrottleCommandPtr safety_command_
Safety command message pointer.
ros::NodeHandle nh_
Ros node handler.
Throttle(const ros::NodeHandle &nh, std::string ip, std::string port)
Class constructor.
boost::asio::io_service io_service_
Input/Output communication service.
int current_mode_
Current working mode.
atlascar_base::ThrottleStatus status_
Status message.
void commandCallback(const atlascar_base::ThrottleCommandPtr &command)
This function handles newly received commands.
TopicQueuePriority< atlascar_base::ThrottleCommandPtr > command_queue_
Command queue holding class.
~Throttle()
Class destructor.
void connectHandler(void)
This function will be called (asynchronously) on a successful connection.
void init()
Initialize the class.
diagnostic_updater::HeaderlessTopicDiagnostic status_freq_
Frequency diagnostics tool.
double status_max_frequency_
Maximum admissible frequency.
void loop()
Start main control loop.
ros::Publisher status_pub_
Ros status publisher.
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
Diagnostics function handler.
std::string server_ip_
Throttle server ip.
AsyncClient comm_
Asynchronous tcp/ip communication object.