38 #include <odometer/OdometerStatus.h>
40 #include <topic_priority/topic_priority.h>
42 #include <diagnostic_updater/diagnostic_updater.h>
43 #include <diagnostic_updater/publisher.h>
45 #include <boost/thread/thread.hpp>
46 #include <boost/format.hpp>
47 #include <boost/spirit/include/qi.hpp>
48 #include <boost/spirit/include/phoenix_core.hpp>
49 #include <boost/spirit/include/phoenix_operator.hpp>
51 #include <tcp_client/AsyncClient.h>
89 Odometer(
const ros::NodeHandle& nh,std::string ip,std::string port):
92 nh_(nh), status_freq_(
"Odometer",updater_,diagnostic_updater::FrequencyStatusParam(&status_max_frequency_,&status_min_frequency_, 0.1, 10)),
93 status_max_frequency_(8.0),
94 status_min_frequency_(10.0),
95 comm_(io_service_,server_ip_,server_port_)
117 updater_.setHardwareID(
"odometer");
121 comm_.readHandler.connect(boost::bind(&Odometer::newData,
this,_1));
124 comm_.connectHandler.connect(boost::bind(&Odometer::connectHandler,
this));
137 status_pub_ = nh_.advertise<odometer::OdometerStatus>(
"status", 1);
153 boost::thread thread(boost::bind(&boost::asio::io_service::run, &io_service_));
187 comm_.write(
"start");
188 }
catch(std::exception& e)
190 std::cout <<
"Exception: " << e.what() <<
"\n";
203 namespace qi = boost::spirit::qi;
207 using qi::ascii::space;
209 using boost::phoenix::ref;
211 data.erase(data.begin());
212 data.erase(data.end()-2,data.end());
214 qi::phrase_parse(data.begin(),data.end(),
215 omit[char_] >> double_[ref(status_.count) = _1] >>
216 omit[char_] >> double_[ref(status_.pulses_sec) = _1] >>
217 omit[char_] >> double_[ref(status_.revolutions_sec) = _1] >>
218 omit[char_] >> double_[ref(status_.velocity) = _1]
220 status_.header.stamp = ros::Time::now();
221 status_.header.frame_id =
"";
223 status_pub_.publish(status_);
232 void diagnostics(diagnostic_updater::DiagnosticStatusWrapper& stat)
234 if(!comm_.isConnected())
236 stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR,
"No connection to hardware.");
237 stat.add(
"Status",comm_.error_.message());
240 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
"No problems.");
241 stat.add(
"Connected with",server_ip_);
242 stat.add(
"Port number",server_port_);
244 boost::format fmt(
"%.2f");
245 fmt % (ros::Time::now()-status_.header.stamp).toSec();
247 stat.add(
"Last message sent", fmt.str()+
" seconds ago" );
odometer::OdometerStatus status_
Status message.
ros::NodeHandle nh_
Ros node handler.
void loop()
Start main control loop.
diagnostic_updater::HeaderlessTopicDiagnostic status_freq_
Frequency diagnostics tool.
void connectHandler(void)
This function will be called (asynchronously) on a successful connection.
boost::asio::io_service io_service_
Input/Output communication service.
std::string server_ip_
Ip of the Arduino server.
void init()
Initialize the class.
ros::Publisher status_pub_
Ros status publisher.
double status_max_frequency_
Maximum admissible frequency.
void newData(string data)
This function will be called (asynchronously) upon arrival of new data.
double status_min_frequency_
Minimum admissible frequency.
diagnostic_updater::Updater updater_
Diagnostics class.
Odometer(const ros::NodeHandle &nh, std::string ip, std::string port)
Class constructor.
AsyncClient comm_
Asynchronous tcp/ip communication object.
~Odometer()
Class destructor.
void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat)
Diagnostics function handler.
std::string server_port_
Port of the Arduino server.
void setupMessaging()
Start ros message subscribing and advertising.