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 <sstream>
00030
00031
00032
00033
00034
00035
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
00046
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
00089
00090
00091 tf::TransformBroadcaster odom_broadcaster;
00092 geometry_msgs::TransformStamped odom_trans;
00093
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
00101 bool sonar_enabled;
00102
00103
00104 bool publish_sonar;
00105 bool publish_sonar_pointcloud2;
00106
00107
00108 bool debug_aria;
00109 std::string aria_log_filename;
00110
00111
00112 int TicksMM, DriftFactor, RevCount;
00113
00114
00115 dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server;
00116 };
00117
00118 void RosAriaNode::readParameters()
00119 {
00120
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
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
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
00264 n = nh;
00265
00266
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
00275 n.param( "debug_aria", debug_aria, false );
00276 n.param( "aria_log_filename", aria_log_filename, std::string("Aria.log") );
00277
00278
00279
00280
00281
00282
00283
00284
00285
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
00293
00294
00295
00296
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 );
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 );
00312 motors_state.data = false;
00313 published_motors_state = false;
00314
00315
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
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
00329 robot->disableMotors();
00330 robot->disableSonar();
00331
00332 robot->stopRunning();
00333 robot->waitForRunExit();
00334 Aria::shutdown();
00335 }
00336
00337 int RosAriaNode::Setup()
00338 {
00339
00340
00341
00342 robot = new ArRobot();
00343 ArArgumentBuilder *args = new ArArgumentBuilder();
00344 ArArgumentParser *argparser = new ArArgumentParser(args);
00345 argparser->loadDefaultArguments();
00346
00347
00348
00349
00350
00351 size_t colon_pos = serial_port.find(":");
00352 if (colon_pos != std::string::npos)
00353 {
00354 args->add("-remoteHost");
00355 args->add(serial_port.substr(0, colon_pos).c_str());
00356 args->add("-remoteRobotTcpPort");
00357 args->add(serial_port.substr(colon_pos+1).c_str());
00358 }
00359 else
00360 {
00361 args->add("-robotPort");
00362 args->add(serial_port.c_str());
00363 }
00364
00365
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
00377 args->add("-robotLogPacketsReceived");
00378 args->add("-robotLogPacketsSent");
00379 args->add("-robotLogVelocitiesReceived");
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
00387 conn = new ArRobotConnector(argparser, robot);
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
00394 if(!Aria::parseArgs())
00395 {
00396 ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
00397 return 1;
00398 }
00399
00400 readParameters();
00401
00402
00403 dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
00404
00405
00406 rosaria::RosAriaConfig dynConf_min;
00407 dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
00408 dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
00409
00410
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
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
00428
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
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
00459 robot->enableMotors();
00460
00461
00462 robot->disableSonar();
00463
00464
00465 robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
00466
00467
00468 bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
00469 bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
00470
00471
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
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);
00488 position.twist.twist.linear.x = robot->getVel()/1000;
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
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
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
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
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
00550
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
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
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
00582 if (publish_sonar || publish_sonar_pointcloud2)
00583 {
00584 sensor_msgs::PointCloud cloud;
00585 cloud.header.stamp = position.header.stamp;
00586
00587 cloud.header.frame_id = frame_id_sonar;
00588
00589
00590 std::stringstream sonar_debug_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
00602 sonar_debug_info << reading->getRange() << " ";
00603
00604
00605
00606
00607
00608
00609
00610
00611
00612
00613
00614
00615
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
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 }
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
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
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 }