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


rosaria
Author(s): Srećko Jurić-Kavelj
autogenerated on Wed Aug 26 2015 16:00:27