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>
00013 #include <sensor_msgs/PointCloud2.h>
00014 #include <sensor_msgs/point_cloud_conversion.h>
00015 #include "nav_msgs/Odometry.h"
00016 #include "rosaria/BumperState.h"
00017 #include "tf/tf.h"
00018 #include "tf/transform_listener.h"
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
00061
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
00107
00108
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
00118 bool sonar_enabled;
00119
00120
00121 bool publish_sonar;
00122 bool publish_sonar_pointcloud2;
00123
00124
00125 bool debug_aria;
00126 std::string aria_log_filename;
00127
00128
00129 int TicksMM, DriftFactor, RevCount;
00130
00131
00132 dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server;
00133
00134
00135 bool publish_aria_lasers;
00136 };
00137
00138 void RosAriaNode::readParameters()
00139 {
00140
00141
00142
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
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
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
00179 }
00180 robot->unlock();
00181 }
00182
00183 void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
00184 {
00185
00186
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
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
00290
00291
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
00300 n.param( "debug_aria", debug_aria, false );
00301 n.param( "aria_log_filename", aria_log_filename, std::string("Aria.log") );
00302
00303
00304 n.param("publish_aria_lasers", publish_aria_lasers, false);
00305
00306
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
00313
00314
00315
00316
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 );
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 );
00332 motors_state.data = false;
00333 published_motors_state = false;
00334
00335
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
00345 robot->disableMotors();
00346 robot->disableSonar();
00347
00348 robot->stopRunning();
00349 robot->waitForRunExit();
00350 Aria::shutdown();
00351 }
00352
00353 int RosAriaNode::Setup()
00354 {
00355
00356
00357
00358 robot = new ArRobot();
00359 ArArgumentBuilder *args = new ArArgumentBuilder();
00360 ArArgumentParser *argparser = new ArArgumentParser(args);
00361 argparser->loadDefaultArguments();
00362
00363
00364
00365
00366
00367 size_t colon_pos = serial_port.find(":");
00368 if (colon_pos != std::string::npos)
00369 {
00370 args->add("-remoteHost");
00371 args->add(serial_port.substr(0, colon_pos).c_str());
00372 args->add("-remoteRobotTcpPort");
00373 args->add(serial_port.substr(colon_pos+1).c_str());
00374 }
00375 else
00376 {
00377 args->add("-robotPort %s", serial_port.c_str());
00378 }
00379
00380
00381 if(serial_baud != 0)
00382 {
00383 args->add("-robotBaud %d", serial_baud);
00384 }
00385
00386 if( debug_aria )
00387 {
00388
00389 args->add("-robotLogPacketsReceived");
00390 args->add("-robotLogPacketsSent");
00391 args->add("-robotLogVelocitiesReceived");
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
00399 conn = new ArRobotConnector(argparser, robot);
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
00409 if(!Aria::parseArgs())
00410 {
00411 ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
00412 return 1;
00413 }
00414
00415 readParameters();
00416
00417
00418 dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
00419
00420
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
00427
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
00469 robot->enableMotors();
00470
00471
00472 robot->disableSonar();
00473
00474
00475 robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
00476
00477
00478 bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
00479 bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
00480
00481
00482 robot->runAsync(true);
00483
00484
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)
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
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
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
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);
00538 position.twist.twist.linear.x = robot->getVel()/1000.0;
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
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
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
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
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
00599
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
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
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
00631 if (publish_sonar || publish_sonar_pointcloud2)
00632 {
00633 sensor_msgs::PointCloud cloud;
00634 cloud.header.stamp = position.header.stamp;
00635
00636 cloud.header.frame_id = frame_id_sonar;
00637
00638
00639 std::stringstream sonar_debug_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
00651 sonar_debug_info << reading->getRange() << " ";
00652
00653
00654
00655
00656
00657
00658
00659
00660
00661
00662
00663
00664
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
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 }
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
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
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
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 }