RosAria.cpp
Go to the documentation of this file.
1 #include <stdio.h>
2 #include <math.h>
3 #ifdef ADEPT_PKG
4  #include <Aria.h>
5  #include <ArRobotConfigPacketReader.h> // todo remove after ArRobotConfig implemented in AriaCoda
6 #else
7  #include <Aria/Aria.h>
8  #include <Aria/ArRobotConfigPacketReader.h> // todo remove after ArRobotConfig implemented in AriaCoda
9 #endif
10 #include "ros/ros.h"
11 #include "geometry_msgs/Twist.h"
12 #include "geometry_msgs/Pose.h"
13 #include "geometry_msgs/PoseStamped.h"
14 #include <sensor_msgs/PointCloud.h> //for sonar data
15 #include <sensor_msgs/PointCloud2.h>
16 #include <sensor_msgs/point_cloud_conversion.h> // can optionally publish sonar as new type pointcloud2
17 #include "nav_msgs/Odometry.h"
18 #include "rosaria/BumperState.h"
19 #include "tf/tf.h"
20 #include "tf/transform_listener.h" //for tf::getPrefixParam
22 #include "tf/transform_datatypes.h"
23 #include <dynamic_reconfigure/server.h>
24 #include <rosaria/RosAriaConfig.h>
25 #include "std_msgs/Float64.h"
26 #include "std_msgs/Float32.h"
27 #include "std_msgs/Int8.h"
28 #include "std_msgs/Bool.h"
29 #include "std_srvs/Empty.h"
30 
31 #include "LaserPublisher.h"
32 
33 #include <sstream>
34 
35 
53 {
54  public:
56  virtual ~RosAriaNode();
57 
58  public:
59  int Setup();
60  void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
61  void cmdvel_watchdog(const ros::TimerEvent& event);
62  //void cmd_enable_motors_cb();
63  //void cmd_disable_motors_cb();
64  void spin();
65  void publish();
66  void sonarConnectCb();
67  void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level);
68  void readParameters();
69 
70  protected:
77 
79  std_msgs::Int8 recharge_state;
80 
82 
84  std_msgs::Bool motors_state;
86 
88 
91  bool enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
92  bool disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
93 
97 
98  std::string serial_port;
100 
101  ArRobotConnector *conn;
102  ArLaserConnector *laserConnector;
103  ArRobot *robot;
104  nav_msgs::Odometry position;
105  rosaria::BumperState bumpers;
106  ArPose pos;
107  ArFunctorC<RosAriaNode> myPublishCB;
108  //ArRobot::ChargeState batteryCharge;
109 
110  //for odom->base_link transform
112  geometry_msgs::TransformStamped odom_trans;
113 
114  std::string frame_id_odom;
115  std::string frame_id_base_link;
116  std::string frame_id_bumper;
117  std::string frame_id_sonar;
118 
119  // flag indicating whether sonar was enabled or disabled on the robot
121 
122  // enable and publish sonar topics. set to true when first subscriber connects, set to false when last subscriber disconnects.
125 
126  // Debug Aria
128  std::string aria_log_filename;
129 
130  // Robot Calibration Parameters (see readParameters() function)
131  int TicksMM, DriftFactor, RevCount; //If TicksMM or RevCount are <0, don't use. If DriftFactor is -99999, don't use (DriftFactor could be 0 or negative).
132 
133  // dynamic_reconfigure
134  dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server;
135 
136  // whether to publish aria lasers
138 };
139 
141 {
142  // Robot Parameters. If a parameter was given and is nonzero, set it now.
143  // Otherwise, get default value for this robot (from getOrigRobotConfig()).
144  // Parameter values are stored in member variables for possible later use by the user with dynamic reconfigure.
145  robot->lock();
146  ros::NodeHandle n_("~");
147  if (n_.getParam("TicksMM", TicksMM) && TicksMM > 0)
148  {
149  ROS_INFO("Setting robot TicksMM from ROS Parameter: %d", TicksMM);
150  robot->comInt(93, TicksMM);
151  }
152  else
153  {
154  TicksMM = robot->getOrigRobotConfig()->getTicksMM();
155  ROS_INFO("This robot's TicksMM parameter: %d", TicksMM);
156  //n_.setParam( "TicksMM", TicksMM);
157  }
158 
159  if (n_.getParam("DriftFactor", DriftFactor) && DriftFactor != -99999)
160  {
161  ROS_INFO("Setting robot DriftFactor from ROS Parameter: %d", DriftFactor);
162  robot->comInt(89, DriftFactor);
163  }
164  else
165  {
166  DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
167  ROS_INFO("This robot's DriftFactor parameter: %d", DriftFactor);
168  //n_.setParam( "DriftFactor", DriftFactor);
169  }
170 
171  if (n_.getParam("RevCount", RevCount) && RevCount > 0)
172  {
173  ROS_INFO("Setting robot RevCount from ROS Parameter: %d", RevCount);
174  robot->comInt(88, RevCount);
175  }
176  else
177  {
178  RevCount = robot->getOrigRobotConfig()->getRevCount();
179  ROS_INFO("This robot's RevCount parameter: %d", RevCount);
180  //n_.setParam( "RevCount", RevCount);
181  }
182  robot->unlock();
183 }
184 
185 void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
186 {
187  //
188  // Odometry Settings
189  //
190  robot->lock();
191  if(TicksMM != config.TicksMM && config.TicksMM > 0)
192  {
193  ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM);
194  TicksMM = config.TicksMM;
195  robot->comInt(93, TicksMM);
196  }
197 
198  if(DriftFactor != config.DriftFactor && config.DriftFactor != -99999)
199  {
200  ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor);
201  DriftFactor = config.DriftFactor;
202  robot->comInt(89, DriftFactor);
203  }
204 
205  if(RevCount != config.RevCount && config.RevCount > 0)
206  {
207  ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount);
208  RevCount = config.RevCount;
209  robot->comInt(88, RevCount);
210  }
211 
212  //
213  // Acceleration Parameters
214  //
215  int value;
216  value = config.trans_accel * 1000;
217  if(value != robot->getTransAccel() && value > 0)
218  {
219  ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %d", value);
220  robot->setTransAccel(value);
221  }
222 
223  value = config.trans_decel * 1000;
224  if(value != robot->getTransDecel() && value > 0)
225  {
226  ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value);
227  robot->setTransDecel(value);
228  }
229 
230  value = config.lat_accel * 1000;
231  if(value != robot->getLatAccel() && value > 0)
232  {
233  ROS_INFO("Setting LatAccel from Dynamic Reconfigure: %d", value);
234  if (robot->getAbsoluteMaxLatAccel() > 0 )
235  robot->setLatAccel(value);
236  }
237 
238  value = config.lat_decel * 1000;
239  if(value != robot->getLatDecel() && value > 0)
240  {
241  ROS_INFO("Setting LatDecel from Dynamic Reconfigure: %d", value);
242  if (robot->getAbsoluteMaxLatDecel() > 0 )
243  robot->setLatDecel(value);
244  }
245 
246  value = config.rot_accel * 180/M_PI;
247  if(value != robot->getRotAccel() && value > 0)
248  {
249  ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %d", value);
250  robot->setRotAccel(value);
251  }
252 
253  value = config.rot_decel * 180/M_PI;
254  if(value != robot->getRotDecel() && value > 0)
255  {
256  ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %d", value);
257  robot->setRotDecel(value);
258  }
259  robot->unlock();
260 }
261 
264 {
267  robot->lock();
269  {
270  robot->enableSonar();
271  sonar_enabled = false;
272  }
274  {
275  robot->disableSonar();
276  sonar_enabled = true;
277  }
278  robot->unlock();
279 }
280 
282  n(nh),
283  serial_port(""), serial_baud(0),
284  conn(NULL), laserConnector(NULL), robot(NULL),
287  debug_aria(false),
288  TicksMM(-1), DriftFactor(-99999), RevCount(-1),
289  publish_aria_lasers(false)
290 {
291  // read in runtime parameters
292 
293  // port and baud
294  n.param( "port", serial_port, std::string("/dev/ttyUSB0") );
295  ROS_INFO( "RosAria: set port: [%s]", serial_port.c_str() );
296 
297  n.param("baud", serial_baud, 0);
298  if(serial_baud != 0)
299  ROS_INFO("RosAria: set serial port baud rate %d", serial_baud);
300 
301  // handle debugging more elegantly
302  n.param( "debug_aria", debug_aria, false ); // default not to debug
303  n.param( "aria_log_filename", aria_log_filename, std::string("Aria.log") );
304 
305  // whether to connect to lasers using aria
306  n.param("publish_aria_lasers", publish_aria_lasers, false);
307 
308  // Get frame_ids to use.
309  n.param("odom_frame", frame_id_odom, std::string("odom"));
310  n.param("base_link_frame", frame_id_base_link, std::string("base_link"));
311  n.param("bumpers_frame", frame_id_bumper, std::string("bumpers"));
312  n.param("sonar_frame", frame_id_sonar, std::string("sonar"));
313 
314  // advertise services for data topics
315  // second argument to advertise() is queue size.
316  // other argmuments (optional) are callbacks, or a boolean "latch" flag (whether to send current data to new
317  // subscribers when they subscribe).
318  // See ros::NodeHandle API docs.
319  pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
320  bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
321  sonar_pub = n.advertise<sensor_msgs::PointCloud>("sonar", 50,
322  boost::bind(&RosAriaNode::sonarConnectCb, this),
323  boost::bind(&RosAriaNode::sonarConnectCb, this));
324  sonar_pointcloud2_pub = n.advertise<sensor_msgs::PointCloud2>("sonar_pointcloud2", 50,
325  boost::bind(&RosAriaNode::sonarConnectCb, this),
326  boost::bind(&RosAriaNode::sonarConnectCb, this));
327 
328  voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
329  recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
330  recharge_state.data = -2;
331  state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
332 
333  motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
334  motors_state.data = false;
335  published_motors_state = false;
336 
337  // advertise enable/disable services
338  enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
339  disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
340 
342 }
343 
345 {
346  // disable motors and sonar.
347  robot->disableMotors();
348  robot->disableSonar();
349 
350  robot->stopRunning();
351  robot->waitForRunExit();
352  Aria::shutdown();
353 }
354 
356 {
357  // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
358  // called once per instance, and these objects need to persist until the process terminates.
359 
360  robot = new ArRobot();
361  ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
362  ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
363  argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
364 
365  // Now add any parameters given via ros params (see RosAriaNode constructor):
366 
367  // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
368  // for wireless serial connection. Otherwise, interpret it as a serial port name.
369  size_t colon_pos = serial_port.find(":");
370  if (colon_pos != std::string::npos)
371  {
372  args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
373  args->add(serial_port.substr(0, colon_pos).c_str());
374  args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
375  args->add(serial_port.substr(colon_pos+1).c_str());
376  }
377  else
378  {
379  args->add("-robotPort %s", serial_port.c_str()); // pass robot's serial port to Aria
380  }
381 
382  // if a baud rate was specified in baud parameter
383  if(serial_baud != 0)
384  {
385  args->add("-robotBaud %d", serial_baud);
386  }
387 
388  if( debug_aria )
389  {
390  // turn on all ARIA debugging
391  args->add("-robotLogPacketsReceived"); // log received packets
392  args->add("-robotLogPacketsSent"); // log sent packets
393  args->add("-robotLogVelocitiesReceived"); // log received velocities
394  args->add("-robotLogMovementSent");
395  args->add("-robotLogMovementReceived");
396  ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
397  }
398 
399 
400  // Connect to the robot
401  conn = new ArRobotConnector(argparser, robot); // warning never freed
402  if (!conn->connectRobot()) {
403  ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
404  return 1;
405  }
406 
408  laserConnector = new ArLaserConnector(argparser, robot, conn);
409 
410  // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
411  if(!Aria::parseArgs())
412  {
413  ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
414  return 1;
415  }
416 
417  readParameters();
418 
419  // Start dynamic_reconfigure server
420  dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
421 
422  // Setup Parameter Minimums and maximums
423  rosaria::RosAriaConfig dynConf_min;
424  rosaria::RosAriaConfig dynConf_max;
425 
426  dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
427  dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
428  // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
429  // Until then, set unit length interval.
430  dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
431  dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
432  dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
433  dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
434 
435  dynConf_min.trans_accel = 0;
436  dynConf_min.trans_decel = 0;
437  dynConf_min.lat_accel = 0;
438  dynConf_min.lat_decel = 0;
439  dynConf_min.rot_accel = 0;
440  dynConf_min.rot_decel = 0;
441 
442  dynConf_min.TicksMM = 0;
443  dynConf_max.TicksMM = 200;
444  dynConf_min.DriftFactor = -99999;
445  dynConf_max.DriftFactor = 32767;
446  dynConf_min.RevCount = 0;
447  dynConf_max.RevCount = 65535;
448 
449  dynamic_reconfigure_server->setConfigMax(dynConf_max);
450  dynamic_reconfigure_server->setConfigMin(dynConf_min);
451 
452 
453  rosaria::RosAriaConfig dynConf_default;
454  dynConf_default.trans_accel = robot->getTransAccel() / 1000;
455  dynConf_default.trans_decel = robot->getTransDecel() / 1000;
456  dynConf_default.lat_accel = robot->getLatAccel() / 1000;
457  dynConf_default.lat_decel = robot->getLatDecel() / 1000;
458  dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180;
459  dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180;
460 
461  dynConf_default.TicksMM = 0;
462  dynConf_default.DriftFactor = -99999;
463  dynConf_default.RevCount = 0;
464 
465  dynamic_reconfigure_server->setConfigDefault(dynConf_default);
466 
467  dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
468 
469 
470  // Enable the motors
471  robot->enableMotors();
472 
473  // disable sonars on startup
474  robot->disableSonar();
475 
476  // callback will be called by ArRobot background processing thread for every SIP data packet received from robot
477  robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
478 
479  // Initialize bumpers with robot number of bumpers
480  bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
481  bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
482 
483  // Run ArRobot background processing thread
484  robot->runAsync(true);
485 
486  // connect to lasers and create publishers
488  {
489  ROS_INFO_NAMED("rosaria", "rosaria: Connecting to laser(s) configured in ARIA parameter file(s)...");
490  if (!laserConnector->connectLasers())
491  {
492  ROS_FATAL_NAMED("rosaria", "rosaria: Error connecting to laser(s)...");
493  return 1;
494  }
495 
496  robot->lock();
497  const std::map<int, ArLaser*> *lasers = robot->getLaserMap();
498  ROS_INFO_NAMED("rosaria", "rosaria: there are %lu connected lasers", lasers->size());
499  for(std::map<int, ArLaser*>::const_iterator i = lasers->begin(); i != lasers->end(); ++i)
500  {
501  ArLaser *l = i->second;
502  int ln = i->first;
503  std::string tfname("laser");
504  if(lasers->size() > 1 || ln > 1) // no number if only one laser which is also laser 1
505  tfname += ln;
506  tfname += "_frame";
507  ROS_INFO_NAMED("rosaria", "rosaria: Creating publisher for laser #%d named %s with tf frame name %s", ln, l->getName(), tfname.c_str());
508  new LaserPublisher(l, n, true, tfname);
509  }
510  robot->unlock();
511  ROS_INFO_NAMED("rosaria", "rosaria: Done creating laser publishers");
512  }
513 
514  // subscribe to command topics
515  cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
516  boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
517 
518  // register a watchdog for cmd_vel timeout
519  double cmdvel_timeout_param = 0.6;
520  n.param("cmd_vel_timeout", cmdvel_timeout_param, 0.6);
521  cmdvel_timeout = ros::Duration(cmdvel_timeout_param);
522  if (cmdvel_timeout_param > 0.0)
524 
525  ROS_INFO_NAMED("rosaria", "rosaria: Setup complete");
526  return 0;
527 }
528 
530 {
531  ros::spin();
532 }
533 
535 {
536  // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
537  pos = robot->getPose();
539  pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
540  position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s.
541  position.twist.twist.linear.y = robot->getLatVel()/1000.0;
542  position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
543 
544  position.header.frame_id = frame_id_odom;
545  position.child_frame_id = frame_id_base_link;
546  position.header.stamp = ros::Time::now();
548 
549  ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, pose y: %f, pose angle: %f; linear vel x: %f, vel y: %f; angular vel z: %f",
550  position.header.stamp.toSec(),
551  (double)position.pose.pose.position.x,
552  (double)position.pose.pose.position.y,
553  (double)position.pose.pose.orientation.w,
554  (double)position.twist.twist.linear.x,
555  (double)position.twist.twist.linear.y,
556  (double)position.twist.twist.angular.z
557  );
558 
559  // publishing transform odom->base_link
560  odom_trans.header.stamp = ros::Time::now();
561  odom_trans.header.frame_id = frame_id_odom;
562  odom_trans.child_frame_id = frame_id_base_link;
563 
564  odom_trans.transform.translation.x = pos.getX()/1000;
565  odom_trans.transform.translation.y = pos.getY()/1000;
566  odom_trans.transform.translation.z = 0.0;
567  odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
568 
570 
571  // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
572  int stall = robot->getStallValue();
573  unsigned char front_bumpers = (unsigned char)(stall >> 8);
574  unsigned char rear_bumpers = (unsigned char)(stall);
575 
576  bumpers.header.frame_id = frame_id_bumper;
577  bumpers.header.stamp = ros::Time::now();
578 
579  std::stringstream bumper_info(std::stringstream::out);
580  // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
581  for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
582  {
583  bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
584  bumper_info << " " << (front_bumpers & (1 << (i+1)));
585  }
586  ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());
587 
588  bumper_info.str("");
589  // Rear bumpers have reverse order (rightmost is LSB)
590  unsigned int numRearBumpers = robot->getNumRearBumpers();
591  for (unsigned int i=0; i<numRearBumpers; i++)
592  {
593  bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
594  bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
595  }
596  ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
597 
599 
600  //Publish battery information
601  // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option
602  std_msgs::Float64 batteryVoltage;
603  batteryVoltage.data = robot->getRealBatteryVoltageNow();
604  voltage_pub.publish(batteryVoltage);
605 
606  if(robot->haveStateOfCharge())
607  {
608  std_msgs::Float32 soc;
609  soc.data = robot->getStateOfCharge()/100.0;
611  }
612 
613  // publish recharge state if changed
614  char s = robot->getChargeState();
615  if(s != recharge_state.data)
616  {
617  ROS_INFO("RosAria: publishing new recharge state %d.", s);
618  recharge_state.data = s;
620  }
621 
622  // publish motors state if changed
623  bool e = robot->areMotorsEnabled();
624  if(e != motors_state.data || !published_motors_state)
625  {
626  ROS_INFO("RosAria: publishing new motors state %d.", e);
627  motors_state.data = e;
629  published_motors_state = true;
630  }
631 
632  // Publish sonar information, if enabled.
634  {
635  sensor_msgs::PointCloud cloud; //sonar readings.
636  cloud.header.stamp = position.header.stamp; //copy time.
637  // sonar sensors relative to base_link
638  cloud.header.frame_id = frame_id_sonar;
639 
640 
641  std::stringstream sonar_debug_info; // Log debugging info
642  sonar_debug_info << "Sonar readings: ";
643 
644  for (int i = 0; i < robot->getNumSonar(); i++) {
645  ArSensorReading* reading = NULL;
646  reading = robot->getSonarReading(i);
647  if(!reading) {
648  ROS_WARN("RosAria: Did not receive a sonar reading.");
649  continue;
650  }
651 
652  // getRange() will return an integer between 0 and 5000 (5m)
653  sonar_debug_info << reading->getRange() << " ";
654 
655  // local (x,y). Appears to be from the centre of the robot, since values may
656  // exceed 5000. This is good, since it means we only need 1 transform.
657  // x & y seem to be swapped though, i.e. if the robot is driving north
658  // x is north/south and y is east/west.
659  //
660  //ArPose sensor = reading->getSensorPosition(); //position of sensor.
661  // sonar_debug_info << "(" << reading->getLocalX()
662  // << ", " << reading->getLocalY()
663  // << ") from (" << sensor.getX() << ", "
664  // << sensor.getY() << ") ;; " ;
665 
666  //add sonar readings (robot-local coordinate frame) to cloud
667  geometry_msgs::Point32 p;
668  p.x = reading->getLocalX() / 1000.0;
669  p.y = reading->getLocalY() / 1000.0;
670  p.z = 0.0;
671  cloud.points.push_back(p);
672  }
673  ROS_DEBUG_STREAM(sonar_debug_info.str());
674 
675  // publish topic(s)
676 
678  {
679  sensor_msgs::PointCloud2 cloud2;
681  {
682  ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
683  }
684  else
685  {
687  }
688  }
689 
690  if(publish_sonar)
691  {
692  sonar_pub.publish(cloud);
693  }
694  } // end if sonar_enabled
695 }
696 
697 bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
698 {
699  ROS_INFO("RosAria: Enable motors request.");
700  robot->lock();
701  if(robot->isEStopPressed())
702  ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
703  robot->enableMotors();
704  robot->unlock();
705  // todo could wait and see if motors do become enabled, and send a response with an error flag if not
706  return true;
707 }
708 
709 bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
710 {
711  ROS_INFO("RosAria: Disable motors request.");
712  robot->lock();
713  robot->disableMotors();
714  robot->unlock();
715  // todo could wait and see if motors do become disabled, and send a response with an error flag if not
716  return true;
717 }
718 
719 void
720 RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
721 {
723  ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
724 
725  robot->lock();
726  robot->setVel(msg->linear.x*1e3);
727  if(robot->hasLatVel())
728  robot->setLatVel(msg->linear.y*1e3);
729  robot->setRotVel(msg->angular.z*180/M_PI);
730  robot->unlock();
731  ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
732  (double) msg->linear.x * 1e3, (double) msg->linear.y * 1e3, (double) msg->angular.z * 180/M_PI);
733 }
734 
736 {
737  // stop robot if no cmd_vel message was received for 0.6 seconds
738  if (ros::Time::now() - veltime > ros::Duration(0.6))
739  {
740  robot->lock();
741  robot->setVel(0.0);
742  if(robot->hasLatVel())
743  robot->setLatVel(0.0);
744  robot->setRotVel(0.0);
745  robot->unlock();
746  }
747 }
748 
749 
750 int main( int argc, char** argv )
751 {
752  ros::init(argc,argv, "RosAria");
753  ros::NodeHandle n(std::string("~"));
754  Aria::init();
755 
756  RosAriaNode *node = new RosAriaNode(n);
757 
758  if( node->Setup() != 0 )
759  {
760  ROS_FATAL( "RosAria: ROS node setup failed... \n" );
761  return -1;
762  }
763 
764  node->spin();
765 
766  delete node;
767 
768  ROS_INFO( "RosAria: Quitting... \n" );
769  return 0;
770 
771 }
bool publish_aria_lasers
Definition: RosAria.cpp:137
int serial_baud
Definition: RosAria.cpp:99
ros::Duration cmdvel_timeout
Definition: RosAria.cpp:96
bool enable_motors_cb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
Definition: RosAria.cpp:697
#define ROS_FATAL(...)
ArPose pos
Definition: RosAria.cpp:106
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
std::string frame_id_bumper
Definition: RosAria.cpp:116
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::ServiceServer enable_srv
Definition: RosAria.cpp:89
geometry_msgs::TransformStamped odom_trans
Definition: RosAria.cpp:112
XmlRpcServer s
ros::Subscriber cmdvel_sub
Definition: RosAria.cpp:87
ArRobot * robot
Definition: RosAria.cpp:103
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void publish()
Definition: RosAria.cpp:534
void cmdvel_cb(const geometry_msgs::TwistConstPtr &)
Definition: RosAria.cpp:720
std::string serial_port
Definition: RosAria.cpp:98
int Setup()
Definition: RosAria.cpp:355
int DriftFactor
Definition: RosAria.cpp:131
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std_msgs::Int8 recharge_state
Definition: RosAria.cpp:79
nav_msgs::Odometry position
Definition: RosAria.cpp:104
#define ROS_WARN(...)
ros::NodeHandle n
Definition: RosAria.cpp:71
std::string aria_log_filename
Definition: RosAria.cpp:128
ros::Time veltime
Definition: RosAria.cpp:94
std::string frame_id_base_link
Definition: RosAria.cpp:115
bool debug_aria
Definition: RosAria.cpp:127
ROSCPP_DECL void spin(Spinner &spinner)
void readParameters()
Definition: RosAria.cpp:140
Node that interfaces between ROS and mobile robot base features via ARIA library. ...
Definition: RosAria.cpp:52
std_msgs::Bool motors_state
Definition: RosAria.cpp:84
bool publish_sonar_pointcloud2
Definition: RosAria.cpp:124
ros::Publisher sonar_pub
Definition: RosAria.cpp:74
RosAriaNode(ros::NodeHandle n)
Definition: RosAria.cpp:281
void spin()
Definition: RosAria.cpp:529
static Quaternion createQuaternionFromYaw(double yaw)
void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
Definition: RosAria.cpp:185
ros::Publisher voltage_pub
Definition: RosAria.cpp:76
bool published_motors_state
Definition: RosAria.cpp:85
std::string frame_id_odom
Definition: RosAria.cpp:114
ArLaserConnector * laserConnector
Definition: RosAria.cpp:102
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool sonar_enabled
Definition: RosAria.cpp:120
void sendTransform(const StampedTransform &transform)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
rosaria::BumperState bumpers
Definition: RosAria.cpp:105
ros::Publisher sonar_pointcloud2_pub
Definition: RosAria.cpp:75
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher pose_pub
Definition: RosAria.cpp:72
#define ROS_DEBUG_STREAM(args)
int main(int argc, char **argv)
Definition: RosAria.cpp:750
ros::ServiceServer disable_srv
Definition: RosAria.cpp:90
uint32_t getNumSubscribers() const
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
static bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
ros::Timer cmdvel_watchdog_timer
Definition: RosAria.cpp:95
ros::Publisher state_of_charge_pub
Definition: RosAria.cpp:81
ArFunctorC< RosAriaNode > myPublishCB
Definition: RosAria.cpp:107
bool disable_motors_cb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
Definition: RosAria.cpp:709
ros::Publisher recharge_state_pub
Definition: RosAria.cpp:78
bool getParam(const std::string &key, std::string &s) const
std::string frame_id_sonar
Definition: RosAria.cpp:117
ros::Publisher bumpers_pub
Definition: RosAria.cpp:73
#define ROS_FATAL_NAMED(name,...)
void cmdvel_watchdog(const ros::TimerEvent &event)
Definition: RosAria.cpp:735
virtual ~RosAriaNode()
Definition: RosAria.cpp:344
static Time now()
bool publish_sonar
Definition: RosAria.cpp:123
void sonarConnectCb()
Called when another node subscribes or unsubscribes from sonar topic.
Definition: RosAria.cpp:263
ros::Publisher motors_state_pub
Definition: RosAria.cpp:83
dynamic_reconfigure::Server< rosaria::RosAriaConfig > * dynamic_reconfigure_server
Definition: RosAria.cpp:134
#define ROS_ERROR(...)
ArRobotConnector * conn
Definition: RosAria.cpp:101
tf::TransformBroadcaster odom_broadcaster
Definition: RosAria.cpp:111
int RevCount
Definition: RosAria.cpp:131
#define ROS_DEBUG(...)


rosaria
Author(s): Srećko Jurić-Kavelj
autogenerated on Thu Mar 18 2021 02:51:25