RosAria.cpp
Go to the documentation of this file.
00001 #include <stdio.h>
00002 #include <math.h>
00003 #ifdef ADEPT_PKG
00004   #include <Aria.h>
00005 #else
00006   #include <Aria/Aria.h>
00007 #endif
00008 #include "ros/ros.h"
00009 #include "geometry_msgs/Twist.h"
00010 #include "geometry_msgs/Pose.h"
00011 #include "geometry_msgs/PoseStamped.h"
00012 #include <sensor_msgs/PointCloud.h>  //for sonar data
00013 #include <sensor_msgs/PointCloud2.h>
00014 #include <sensor_msgs/point_cloud_conversion.h> // can optionally publish sonar as new type pointcloud2
00015 #include "nav_msgs/Odometry.h"
00016 #include "rosaria/BumperState.h"
00017 #include "tf/tf.h"
00018 #include "tf/transform_listener.h"  //for tf::getPrefixParam
00019 #include <tf/transform_broadcaster.h>
00020 #include "tf/transform_datatypes.h"
00021 #include <dynamic_reconfigure/server.h>
00022 #include <rosaria/RosAriaConfig.h>
00023 #include "std_msgs/Float64.h"
00024 #include "std_msgs/Float32.h"
00025 #include "std_msgs/Int8.h"
00026 #include "std_msgs/Bool.h"
00027 #include "std_srvs/Empty.h"
00028 
00029 #include "LaserPublisher.h"
00030 
00031 #include <sstream>
00032 
00033 
00050 class RosAriaNode
00051 {
00052   public:
00053     RosAriaNode(ros::NodeHandle n);
00054     virtual ~RosAriaNode();
00055     
00056   public:
00057     int Setup();
00058     void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
00059     void cmdvel_watchdog(const ros::TimerEvent& event);
00060     //void cmd_enable_motors_cb();
00061     //void cmd_disable_motors_cb();
00062     void spin();
00063     void publish();
00064     void sonarConnectCb();
00065     void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level);
00066     void readParameters();
00067 
00068   protected:
00069     ros::NodeHandle n;
00070     ros::Publisher pose_pub;
00071     ros::Publisher bumpers_pub;
00072     ros::Publisher sonar_pub;
00073     ros::Publisher sonar_pointcloud2_pub;
00074     ros::Publisher voltage_pub;
00075 
00076     ros::Publisher recharge_state_pub;
00077     std_msgs::Int8 recharge_state;
00078 
00079     ros::Publisher state_of_charge_pub;
00080 
00081     ros::Publisher motors_state_pub;
00082     std_msgs::Bool motors_state;
00083     bool published_motors_state;
00084 
00085     ros::Subscriber cmdvel_sub;
00086 
00087     ros::ServiceServer enable_srv;
00088     ros::ServiceServer disable_srv;
00089     bool enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
00090     bool disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
00091 
00092     ros::Time veltime;
00093     ros::Timer cmdvel_watchdog_timer;
00094     ros::Duration cmdvel_timeout;
00095 
00096     std::string serial_port;
00097     int serial_baud;
00098 
00099     ArRobotConnector *conn;
00100     ArLaserConnector *laserConnector;
00101     ArRobot *robot;
00102     nav_msgs::Odometry position;
00103     rosaria::BumperState bumpers;
00104     ArPose pos;
00105     ArFunctorC<RosAriaNode> myPublishCB;
00106     //ArRobot::ChargeState batteryCharge;
00107 
00108     //for odom->base_link transform
00109     tf::TransformBroadcaster odom_broadcaster;
00110     geometry_msgs::TransformStamped odom_trans;
00111     
00112     std::string frame_id_odom;
00113     std::string frame_id_base_link;
00114     std::string frame_id_bumper;
00115     std::string frame_id_sonar;
00116 
00117     // flag indicating whether sonar was enabled or disabled on the robot
00118     bool sonar_enabled; 
00119 
00120     // enable and publish sonar topics. set to true when first subscriber connects, set to false when last subscriber disconnects. 
00121     bool publish_sonar; 
00122     bool publish_sonar_pointcloud2;
00123 
00124     // Debug Aria
00125     bool debug_aria;
00126     std::string aria_log_filename;
00127     
00128     // Robot Calibration Parameters (see readParameters() function)
00129     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).
00130     
00131     // dynamic_reconfigure
00132     dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server;
00133 
00134     // whether to publish aria lasers
00135     bool publish_aria_lasers;
00136 };
00137 
00138 void RosAriaNode::readParameters()
00139 {
00140   // Robot Parameters. If a parameter was given and is nonzero, set it now.
00141   // Otherwise, get default value for this robot (from getOrigRobotConfig()).
00142   // Parameter values are stored in member variables for possible later use by the user with dynamic reconfigure.
00143   robot->lock();
00144   ros::NodeHandle n_("~");
00145   if (n_.getParam("TicksMM", TicksMM) && TicksMM > 0)
00146   {
00147     ROS_INFO("Setting robot TicksMM from ROS Parameter: %d", TicksMM);
00148     robot->comInt(93, TicksMM);
00149   }
00150   else
00151   {
00152     TicksMM = robot->getOrigRobotConfig()->getTicksMM();
00153     ROS_INFO("This robot's TicksMM parameter: %d", TicksMM);
00154     //n_.setParam( "TicksMM", TicksMM);
00155   }
00156   
00157   if (n_.getParam("DriftFactor", DriftFactor) && DriftFactor != -99999)
00158   {
00159     ROS_INFO("Setting robot DriftFactor from ROS Parameter: %d", DriftFactor);
00160     robot->comInt(89, DriftFactor);
00161   }
00162   else
00163   {
00164     DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
00165     ROS_INFO("This robot's DriftFactor parameter: %d", DriftFactor);
00166     //n_.setParam( "DriftFactor", DriftFactor);
00167   }
00168   
00169   if (n_.getParam("RevCount", RevCount) && RevCount > 0)
00170   {
00171     ROS_INFO("Setting robot RevCount from ROS Parameter: %d", RevCount);
00172     robot->comInt(88, RevCount);
00173   }
00174   else
00175   {
00176     RevCount = robot->getOrigRobotConfig()->getRevCount();
00177     ROS_INFO("This robot's RevCount parameter: %d", RevCount);
00178     //n_.setParam( "RevCount", RevCount);
00179   }
00180   robot->unlock();
00181 }
00182 
00183 void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
00184 {
00185   //
00186   // Odometry Settings
00187   //
00188   robot->lock();
00189   if(TicksMM != config.TicksMM && config.TicksMM > 0)
00190   {
00191     ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM);
00192     TicksMM = config.TicksMM;
00193     robot->comInt(93, TicksMM);
00194   }
00195   
00196   if(DriftFactor != config.DriftFactor && config.DriftFactor != -99999) 
00197   {
00198     ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor);
00199     DriftFactor = config.DriftFactor;
00200     robot->comInt(89, DriftFactor);
00201   }
00202   
00203   if(RevCount != config.RevCount && config.RevCount > 0)
00204   {
00205     ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount);
00206     RevCount = config.RevCount;
00207     robot->comInt(88, RevCount);
00208   }
00209   
00210   //
00211   // Acceleration Parameters
00212   //
00213   int value;
00214   value = config.trans_accel * 1000;
00215   if(value != robot->getTransAccel() && value > 0)
00216   {
00217     ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %d", value);
00218     robot->setTransAccel(value);
00219   }
00220   
00221   value = config.trans_decel * 1000;
00222   if(value != robot->getTransDecel() && value > 0)
00223   {
00224     ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value);
00225     robot->setTransDecel(value);
00226   } 
00227   
00228   value = config.lat_accel * 1000;
00229   if(value != robot->getLatAccel() && value > 0)
00230   {
00231     ROS_INFO("Setting LatAccel from Dynamic Reconfigure: %d", value);
00232     if (robot->getAbsoluteMaxLatAccel() > 0 )
00233       robot->setLatAccel(value);
00234   }
00235   
00236   value = config.lat_decel * 1000;
00237   if(value != robot->getLatDecel() && value > 0)
00238   {
00239     ROS_INFO("Setting LatDecel from Dynamic Reconfigure: %d", value);
00240     if (robot->getAbsoluteMaxLatDecel() > 0 )
00241       robot->setLatDecel(value);
00242   }
00243   
00244   value = config.rot_accel * 180/M_PI;
00245   if(value != robot->getRotAccel() && value > 0)
00246   {
00247     ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %d", value);
00248     robot->setRotAccel(value);
00249   }
00250   
00251   value = config.rot_decel * 180/M_PI;
00252   if(value != robot->getRotDecel() && value > 0)
00253   {
00254     ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %d", value);
00255     robot->setRotDecel(value);
00256   } 
00257   robot->unlock();
00258 }
00259 
00261 void RosAriaNode::sonarConnectCb()
00262 {
00263   publish_sonar = (sonar_pub.getNumSubscribers() > 0);
00264   publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0);
00265   robot->lock();
00266   if (publish_sonar || publish_sonar_pointcloud2)
00267   {
00268     robot->enableSonar();
00269     sonar_enabled = false;
00270   }
00271   else if(!publish_sonar && !publish_sonar_pointcloud2)
00272   {
00273     robot->disableSonar();
00274     sonar_enabled = true;
00275   }
00276   robot->unlock();
00277 }
00278 
00279 RosAriaNode::RosAriaNode(ros::NodeHandle nh) : 
00280   n(nh),
00281   serial_port(""), serial_baud(0), 
00282   conn(NULL), laserConnector(NULL), robot(NULL),
00283   myPublishCB(this, &RosAriaNode::publish),
00284   sonar_enabled(false), publish_sonar(false), publish_sonar_pointcloud2(false),
00285   debug_aria(false), 
00286   TicksMM(-1), DriftFactor(-99999), RevCount(-1),
00287   publish_aria_lasers(false)
00288 {
00289   // read in runtime parameters
00290 
00291   // port and baud
00292   n.param( "port", serial_port, std::string("/dev/ttyUSB0") );
00293   ROS_INFO( "RosAria: set port: [%s]", serial_port.c_str() );
00294 
00295   n.param("baud", serial_baud, 0);
00296   if(serial_baud != 0)
00297     ROS_INFO("RosAria: set serial port baud rate %d", serial_baud);
00298 
00299   // handle debugging more elegantly
00300   n.param( "debug_aria", debug_aria, false ); // default not to debug
00301   n.param( "aria_log_filename", aria_log_filename, std::string("Aria.log") );
00302 
00303   // whether to connect to lasers using aria
00304   n.param("publish_aria_lasers", publish_aria_lasers, false);
00305 
00306   // Get frame_ids to use.
00307   n.param("odom_frame", frame_id_odom, std::string("odom"));
00308   n.param("base_link_frame", frame_id_base_link, std::string("base_link"));
00309   n.param("bumpers_frame", frame_id_bumper, std::string("bumpers"));
00310   n.param("sonar_frame", frame_id_sonar, std::string("sonar"));
00311 
00312   // advertise services for data topics
00313   // second argument to advertise() is queue size.
00314   // other argmuments (optional) are callbacks, or a boolean "latch" flag (whether to send current data to new
00315   // subscribers when they subscribe).
00316   // See ros::NodeHandle API docs.
00317   pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
00318   bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
00319   sonar_pub = n.advertise<sensor_msgs::PointCloud>("sonar", 50, 
00320       boost::bind(&RosAriaNode::sonarConnectCb, this),
00321       boost::bind(&RosAriaNode::sonarConnectCb, this));
00322   sonar_pointcloud2_pub = n.advertise<sensor_msgs::PointCloud2>("sonar_pointcloud2", 50,
00323       boost::bind(&RosAriaNode::sonarConnectCb, this),
00324       boost::bind(&RosAriaNode::sonarConnectCb, this));
00325 
00326   voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
00327   recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
00328   recharge_state.data = -2;
00329   state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
00330 
00331   motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
00332   motors_state.data = false;
00333   published_motors_state = false;
00334 
00335   // advertise enable/disable services
00336   enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
00337   disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
00338   
00339   veltime = ros::Time::now();
00340 }
00341 
00342 RosAriaNode::~RosAriaNode()
00343 {
00344   // disable motors and sonar.
00345   robot->disableMotors();
00346   robot->disableSonar();
00347 
00348   robot->stopRunning();
00349   robot->waitForRunExit();
00350   Aria::shutdown();
00351 }
00352 
00353 int RosAriaNode::Setup()
00354 {
00355   // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
00356   // called once per instance, and these objects need to persist until the process terminates.
00357 
00358   robot = new ArRobot();
00359   ArArgumentBuilder *args = new ArArgumentBuilder(); //  never freed
00360   ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
00361   argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
00362 
00363   // Now add any parameters given via ros params (see RosAriaNode constructor):
00364 
00365   // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
00366   // for wireless serial connection. Otherwise, interpret it as a serial port name.
00367   size_t colon_pos = serial_port.find(":");
00368   if (colon_pos != std::string::npos)
00369   {
00370     args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
00371     args->add(serial_port.substr(0, colon_pos).c_str());
00372     args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
00373     args->add(serial_port.substr(colon_pos+1).c_str());
00374   }
00375   else
00376   {
00377     args->add("-robotPort %s", serial_port.c_str()); // pass robot's serial port to Aria
00378   }
00379 
00380   // if a baud rate was specified in baud parameter
00381   if(serial_baud != 0)
00382   {
00383     args->add("-robotBaud %d", serial_baud);
00384   }
00385   
00386   if( debug_aria )
00387   {
00388     // turn on all ARIA debugging
00389     args->add("-robotLogPacketsReceived"); // log received packets
00390     args->add("-robotLogPacketsSent"); // log sent packets
00391     args->add("-robotLogVelocitiesReceived"); // log received velocities
00392     args->add("-robotLogMovementSent");
00393     args->add("-robotLogMovementReceived");
00394     ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
00395   }
00396 
00397 
00398   // Connect to the robot
00399   conn = new ArRobotConnector(argparser, robot); // warning never freed
00400   if (!conn->connectRobot()) {
00401     ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
00402     return 1;
00403   }
00404 
00405   if(publish_aria_lasers)
00406     laserConnector = new ArLaserConnector(argparser, robot, conn);
00407 
00408   // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
00409   if(!Aria::parseArgs())
00410   {
00411     ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
00412     return 1;
00413   }
00414 
00415   readParameters();
00416 
00417   // Start dynamic_reconfigure server
00418   dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
00419   
00420   // Setup Parameter Minimums and maximums
00421   rosaria::RosAriaConfig dynConf_min;
00422   rosaria::RosAriaConfig dynConf_max;
00423   
00424   dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
00425   dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
00426   // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
00427   // Until then, set unit length interval.
00428   dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
00429   dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
00430   dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
00431   dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
00432 
00433   dynConf_min.trans_accel = 0;
00434   dynConf_min.trans_decel = 0;
00435   dynConf_min.lat_accel = 0;
00436   dynConf_min.lat_decel = 0;
00437   dynConf_min.rot_accel = 0;
00438   dynConf_min.rot_decel = 0;
00439   
00440   dynConf_min.TicksMM     = 0;
00441   dynConf_max.TicksMM     = 200;
00442   dynConf_min.DriftFactor = -99999;
00443   dynConf_max.DriftFactor = 32767;
00444   dynConf_min.RevCount    = 0;
00445   dynConf_max.RevCount    = 65535;
00446 
00447   dynamic_reconfigure_server->setConfigMax(dynConf_max);
00448   dynamic_reconfigure_server->setConfigMin(dynConf_min);
00449   
00450   
00451   rosaria::RosAriaConfig dynConf_default;
00452   dynConf_default.trans_accel = robot->getTransAccel() / 1000;
00453   dynConf_default.trans_decel = robot->getTransDecel() / 1000;
00454   dynConf_default.lat_accel   = robot->getLatAccel() / 1000;
00455   dynConf_default.lat_decel   = robot->getLatDecel() / 1000;
00456   dynConf_default.rot_accel   = robot->getRotAccel() * M_PI/180;
00457   dynConf_default.rot_decel   = robot->getRotDecel() * M_PI/180;
00458 
00459   dynConf_default.TicksMM     = 0;
00460   dynConf_default.DriftFactor = -99999;
00461   dynConf_default.RevCount    = 0;
00462   
00463   dynamic_reconfigure_server->setConfigDefault(dynConf_default);
00464   
00465   dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
00466 
00467 
00468   // Enable the motors
00469   robot->enableMotors();
00470 
00471   // disable sonars on startup
00472   robot->disableSonar();
00473 
00474   // callback will  be called by ArRobot background processing thread for every SIP data packet received from robot
00475   robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
00476 
00477   // Initialize bumpers with robot number of bumpers
00478   bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
00479   bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
00480 
00481   // Run ArRobot background processing thread
00482   robot->runAsync(true);
00483 
00484   // connect to lasers and create publishers
00485   if(publish_aria_lasers)
00486   {
00487     ROS_INFO_NAMED("rosaria", "rosaria: Connecting to laser(s) configured in ARIA parameter file(s)...");
00488     if (!laserConnector->connectLasers())
00489     {
00490       ROS_FATAL_NAMED("rosaria", "rosaria: Error connecting to laser(s)...");
00491       return 1;
00492     }
00493 
00494     robot->lock();
00495     const std::map<int, ArLaser*> *lasers = robot->getLaserMap();
00496     ROS_INFO_NAMED("rosaria", "rosaria: there are %lu connected lasers", lasers->size());
00497     for(std::map<int, ArLaser*>::const_iterator i = lasers->begin(); i != lasers->end(); ++i)
00498     {
00499       ArLaser *l = i->second;
00500       int ln = i->first;
00501       std::string tfname("laser");
00502       if(lasers->size() > 1 || ln > 1) // no number if only one laser which is also laser 1
00503         tfname += ln; 
00504       tfname += "_frame";
00505       ROS_INFO_NAMED("rosaria", "rosaria: Creating publisher for laser #%d named %s with tf frame name %s", ln, l->getName(), tfname.c_str());
00506       new LaserPublisher(l, n, true, tfname);
00507     }
00508     robot->unlock();
00509     ROS_INFO_NAMED("rosaria", "rosaria: Done creating laser publishers");
00510   }
00511     
00512   // subscribe to command topics
00513   cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
00514       boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
00515 
00516   // register a watchdog for cmd_vel timeout
00517   double cmdvel_timeout_param = 0.6;
00518   n.param("cmd_vel_timeout", cmdvel_timeout_param, 0.6);
00519   cmdvel_timeout = ros::Duration(cmdvel_timeout_param);
00520   if (cmdvel_timeout_param > 0.0)
00521     cmdvel_watchdog_timer = n.createTimer(ros::Duration(0.1), &RosAriaNode::cmdvel_watchdog, this);
00522 
00523   ROS_INFO_NAMED("rosaria", "rosaria: Setup complete");
00524   return 0;
00525 }
00526 
00527 void RosAriaNode::spin()
00528 {
00529   ros::spin();
00530 }
00531 
00532 void RosAriaNode::publish()
00533 {
00534   // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
00535   pos = robot->getPose();
00536   tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
00537     pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
00538   position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s.
00539   position.twist.twist.linear.y = robot->getLatVel()/1000.0;
00540   position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
00541   
00542   position.header.frame_id = frame_id_odom;
00543   position.child_frame_id = frame_id_base_link;
00544   position.header.stamp = ros::Time::now();
00545   pose_pub.publish(position);
00546 
00547   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", 
00548     position.header.stamp.toSec(), 
00549     (double)position.pose.pose.position.x,
00550     (double)position.pose.pose.position.y,
00551     (double)position.pose.pose.orientation.w,
00552     (double)position.twist.twist.linear.x,
00553     (double)position.twist.twist.linear.y,
00554     (double)position.twist.twist.angular.z
00555   );
00556 
00557   // publishing transform odom->base_link
00558   odom_trans.header.stamp = ros::Time::now();
00559   odom_trans.header.frame_id = frame_id_odom;
00560   odom_trans.child_frame_id = frame_id_base_link;
00561   
00562   odom_trans.transform.translation.x = pos.getX()/1000;
00563   odom_trans.transform.translation.y = pos.getY()/1000;
00564   odom_trans.transform.translation.z = 0.0;
00565   odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
00566   
00567   odom_broadcaster.sendTransform(odom_trans);
00568   
00569   // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
00570   int stall = robot->getStallValue();
00571   unsigned char front_bumpers = (unsigned char)(stall >> 8);
00572   unsigned char rear_bumpers = (unsigned char)(stall);
00573 
00574   bumpers.header.frame_id = frame_id_bumper;
00575   bumpers.header.stamp = ros::Time::now();
00576 
00577   std::stringstream bumper_info(std::stringstream::out);
00578   // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
00579   for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
00580   {
00581     bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
00582     bumper_info << " " << (front_bumpers & (1 << (i+1)));
00583   }
00584   ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());
00585 
00586   bumper_info.str("");
00587   // Rear bumpers have reverse order (rightmost is LSB)
00588   unsigned int numRearBumpers = robot->getNumRearBumpers();
00589   for (unsigned int i=0; i<numRearBumpers; i++)
00590   {
00591     bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
00592     bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
00593   }
00594   ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
00595   
00596   bumpers_pub.publish(bumpers);
00597 
00598   //Publish battery information
00599   // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V)  is a better option
00600   std_msgs::Float64 batteryVoltage;
00601   batteryVoltage.data = robot->getRealBatteryVoltageNow();
00602   voltage_pub.publish(batteryVoltage);
00603 
00604   if(robot->haveStateOfCharge())
00605   {
00606     std_msgs::Float32 soc;
00607     soc.data = robot->getStateOfCharge()/100.0;
00608     state_of_charge_pub.publish(soc);
00609   }
00610 
00611   // publish recharge state if changed
00612   char s = robot->getChargeState();
00613   if(s != recharge_state.data)
00614   {
00615     ROS_INFO("RosAria: publishing new recharge state %d.", s);
00616     recharge_state.data = s;
00617     recharge_state_pub.publish(recharge_state);
00618   }
00619 
00620   // publish motors state if changed
00621   bool e = robot->areMotorsEnabled();
00622   if(e != motors_state.data || !published_motors_state)
00623   {
00624         ROS_INFO("RosAria: publishing new motors state %d.", e);
00625         motors_state.data = e;
00626         motors_state_pub.publish(motors_state);
00627         published_motors_state = true;
00628   }
00629 
00630   // Publish sonar information, if enabled.
00631   if (publish_sonar || publish_sonar_pointcloud2)
00632   {
00633     sensor_msgs::PointCloud cloud;      //sonar readings.
00634     cloud.header.stamp = position.header.stamp; //copy time.
00635     // sonar sensors relative to base_link
00636     cloud.header.frame_id = frame_id_sonar;
00637   
00638 
00639     std::stringstream sonar_debug_info; // Log debugging info
00640     sonar_debug_info << "Sonar readings: ";
00641 
00642     for (int i = 0; i < robot->getNumSonar(); i++) {
00643       ArSensorReading* reading = NULL;
00644       reading = robot->getSonarReading(i);
00645       if(!reading) {
00646         ROS_WARN("RosAria: Did not receive a sonar reading.");
00647         continue;
00648       }
00649     
00650       // getRange() will return an integer between 0 and 5000 (5m)
00651       sonar_debug_info << reading->getRange() << " ";
00652 
00653       // local (x,y). Appears to be from the centre of the robot, since values may
00654       // exceed 5000. This is good, since it means we only need 1 transform.
00655       // x & y seem to be swapped though, i.e. if the robot is driving north
00656       // x is north/south and y is east/west.
00657       //
00658       //ArPose sensor = reading->getSensorPosition();  //position of sensor.
00659       // sonar_debug_info << "(" << reading->getLocalX() 
00660       //                  << ", " << reading->getLocalY()
00661       //                  << ") from (" << sensor.getX() << ", " 
00662       //                  << sensor.getY() << ") ;; " ;
00663     
00664       //add sonar readings (robot-local coordinate frame) to cloud
00665       geometry_msgs::Point32 p;
00666       p.x = reading->getLocalX() / 1000.0;
00667       p.y = reading->getLocalY() / 1000.0;
00668       p.z = 0.0;
00669       cloud.points.push_back(p);
00670     }
00671     ROS_DEBUG_STREAM(sonar_debug_info.str());
00672     
00673     // publish topic(s)
00674 
00675     if(publish_sonar_pointcloud2)
00676     {
00677       sensor_msgs::PointCloud2 cloud2;
00678       if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2))
00679       {
00680         ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
00681       }
00682       else
00683       {
00684         sonar_pointcloud2_pub.publish(cloud2);
00685       }
00686     }
00687 
00688     if(publish_sonar)
00689     {
00690       sonar_pub.publish(cloud);
00691     }
00692   } // end if sonar_enabled
00693 }
00694 
00695 bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
00696 {
00697     ROS_INFO("RosAria: Enable motors request.");
00698     robot->lock();
00699     if(robot->isEStopPressed())
00700         ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
00701     robot->enableMotors();
00702     robot->unlock();
00703         // todo could wait and see if motors do become enabled, and send a response with an error flag if not
00704     return true;
00705 }
00706 
00707 bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
00708 {
00709     ROS_INFO("RosAria: Disable motors request.");
00710     robot->lock();
00711     robot->disableMotors();
00712     robot->unlock();
00713         // todo could wait and see if motors do become disabled, and send a response with an error flag if not
00714     return true;
00715 }
00716 
00717 void
00718 RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
00719 {
00720   veltime = ros::Time::now();
00721   ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
00722 
00723   robot->lock();
00724   robot->setVel(msg->linear.x*1e3);
00725   if(robot->hasLatVel())
00726     robot->setLatVel(msg->linear.y*1e3);
00727   robot->setRotVel(msg->angular.z*180/M_PI);
00728   robot->unlock();
00729   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(),
00730     (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
00731 }
00732 
00733 void RosAriaNode::cmdvel_watchdog(const ros::TimerEvent& event)
00734 {
00735   // stop robot if no cmd_vel message was received for 0.6 seconds
00736   if (ros::Time::now() - veltime > ros::Duration(0.6))
00737   {
00738     robot->lock();
00739     robot->setVel(0.0);
00740     if(robot->hasLatVel())
00741       robot->setLatVel(0.0);
00742     robot->setRotVel(0.0);
00743     robot->unlock();
00744   }
00745 }
00746 
00747 
00748 int main( int argc, char** argv )
00749 {
00750   ros::init(argc,argv, "RosAria");
00751   ros::NodeHandle n(std::string("~"));
00752   Aria::init();
00753 
00754   RosAriaNode *node = new RosAriaNode(n);
00755 
00756   if( node->Setup() != 0 )
00757   {
00758     ROS_FATAL( "RosAria: ROS node setup failed... \n" );
00759     return -1;
00760   }
00761 
00762   node->spin();
00763 
00764   delete node;
00765 
00766   ROS_INFO( "RosAria: Quitting... \n" );
00767   return 0;
00768   
00769 }


rosaria
Author(s): Srećko Jurić-Kavelj
autogenerated on Sat May 13 2017 02:58:12