5 #include <ArRobotConfigPacketReader.h> 8 #include <Aria/ArRobotConfigPacketReader.h> 11 #include "geometry_msgs/Twist.h" 12 #include "geometry_msgs/Pose.h" 13 #include "geometry_msgs/PoseStamped.h" 14 #include <sensor_msgs/PointCloud.h> 15 #include <sensor_msgs/PointCloud2.h> 17 #include "nav_msgs/Odometry.h" 18 #include "rosaria/BumperState.h" 23 #include <dynamic_reconfigure/server.h> 24 #include <rosaria/RosAriaConfig.h> 25 #include "std_msgs/Float64.h" 26 #include "std_msgs/Float32.h" 27 #include "std_msgs/Int8.h" 28 #include "std_msgs/Bool.h" 29 #include "std_srvs/Empty.h" 60 void cmdvel_cb(
const geometry_msgs::TwistConstPtr &);
91 bool enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
92 bool disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
191 if(
TicksMM != config.TicksMM && config.TicksMM > 0)
193 ROS_INFO(
"Setting TicksMM from Dynamic Reconfigure: %d -> %d ",
TicksMM, config.TicksMM);
198 if(
DriftFactor != config.DriftFactor && config.DriftFactor != -99999)
200 ROS_INFO(
"Setting DriftFactor from Dynamic Reconfigure: %d -> %d ",
DriftFactor, config.DriftFactor);
205 if(
RevCount != config.RevCount && config.RevCount > 0)
207 ROS_INFO(
"Setting RevCount from Dynamic Reconfigure: %d -> %d ",
RevCount, config.RevCount);
216 value = config.trans_accel * 1000;
217 if(value !=
robot->getTransAccel() && value > 0)
219 ROS_INFO(
"Setting TransAccel from Dynamic Reconfigure: %d", value);
220 robot->setTransAccel(value);
223 value = config.trans_decel * 1000;
224 if(value !=
robot->getTransDecel() && value > 0)
226 ROS_INFO(
"Setting TransDecel from Dynamic Reconfigure: %d", value);
227 robot->setTransDecel(value);
230 value = config.lat_accel * 1000;
231 if(value !=
robot->getLatAccel() && value > 0)
233 ROS_INFO(
"Setting LatAccel from Dynamic Reconfigure: %d", value);
234 if (
robot->getAbsoluteMaxLatAccel() > 0 )
235 robot->setLatAccel(value);
238 value = config.lat_decel * 1000;
239 if(value !=
robot->getLatDecel() && value > 0)
241 ROS_INFO(
"Setting LatDecel from Dynamic Reconfigure: %d", value);
242 if (
robot->getAbsoluteMaxLatDecel() > 0 )
243 robot->setLatDecel(value);
246 value = config.rot_accel * 180/M_PI;
247 if(value !=
robot->getRotAccel() && value > 0)
249 ROS_INFO(
"Setting RotAccel from Dynamic Reconfigure: %d", value);
250 robot->setRotAccel(value);
253 value = config.rot_decel * 180/M_PI;
254 if(value !=
robot->getRotDecel() && value > 0)
256 ROS_INFO(
"Setting RotDecel from Dynamic Reconfigure: %d", value);
257 robot->setRotDecel(value);
270 robot->enableSonar();
275 robot->disableSonar();
347 robot->disableMotors();
348 robot->disableSonar();
350 robot->stopRunning();
351 robot->waitForRunExit();
360 robot =
new ArRobot();
361 ArArgumentBuilder *
args =
new ArArgumentBuilder();
362 ArArgumentParser *argparser =
new ArArgumentParser(args);
363 argparser->loadDefaultArguments();
370 if (colon_pos != std::string::npos)
372 args->add(
"-remoteHost");
373 args->add(
serial_port.substr(0, colon_pos).c_str());
374 args->add(
"-remoteRobotTcpPort");
375 args->add(
serial_port.substr(colon_pos+1).c_str());
391 args->add(
"-robotLogPacketsReceived");
392 args->add(
"-robotLogPacketsSent");
393 args->add(
"-robotLogVelocitiesReceived");
394 args->add(
"-robotLogMovementSent");
395 args->add(
"-robotLogMovementReceived");
401 conn =
new ArRobotConnector(argparser,
robot);
402 if (!conn->connectRobot()) {
403 ROS_ERROR(
"RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
411 if(!Aria::parseArgs())
413 ROS_ERROR(
"RosAria: ARIA error parsing ARIA startup parameters!");
423 rosaria::RosAriaConfig dynConf_min;
424 rosaria::RosAriaConfig dynConf_max;
426 dynConf_max.trans_accel =
robot->getAbsoluteMaxTransAccel() / 1000;
427 dynConf_max.trans_decel =
robot->getAbsoluteMaxTransDecel() / 1000;
430 dynConf_max.lat_accel = ((
robot->getAbsoluteMaxLatAccel() > 0.0) ?
robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
431 dynConf_max.lat_decel = ((
robot->getAbsoluteMaxLatDecel() > 0.0) ?
robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
432 dynConf_max.rot_accel =
robot->getAbsoluteMaxRotAccel() * M_PI/180;
433 dynConf_max.rot_decel =
robot->getAbsoluteMaxRotDecel() * M_PI/180;
435 dynConf_min.trans_accel = 0;
436 dynConf_min.trans_decel = 0;
437 dynConf_min.lat_accel = 0;
438 dynConf_min.lat_decel = 0;
439 dynConf_min.rot_accel = 0;
440 dynConf_min.rot_decel = 0;
442 dynConf_min.TicksMM = 0;
443 dynConf_max.TicksMM = 200;
444 dynConf_min.DriftFactor = -99999;
445 dynConf_max.DriftFactor = 32767;
446 dynConf_min.RevCount = 0;
447 dynConf_max.RevCount = 65535;
453 rosaria::RosAriaConfig dynConf_default;
454 dynConf_default.trans_accel =
robot->getTransAccel() / 1000;
455 dynConf_default.trans_decel =
robot->getTransDecel() / 1000;
456 dynConf_default.lat_accel =
robot->getLatAccel() / 1000;
457 dynConf_default.lat_decel =
robot->getLatDecel() / 1000;
458 dynConf_default.rot_accel =
robot->getRotAccel() * M_PI/180;
459 dynConf_default.rot_decel =
robot->getRotDecel() * M_PI/180;
461 dynConf_default.TicksMM = 0;
462 dynConf_default.DriftFactor = -99999;
463 dynConf_default.RevCount = 0;
471 robot->enableMotors();
474 robot->disableSonar();
480 bumpers.front_bumpers.resize(
robot->getNumFrontBumpers());
481 bumpers.rear_bumpers.resize(
robot->getNumRearBumpers());
484 robot->runAsync(
true);
489 ROS_INFO_NAMED(
"rosaria",
"rosaria: Connecting to laser(s) configured in ARIA parameter file(s)...");
492 ROS_FATAL_NAMED(
"rosaria",
"rosaria: Error connecting to laser(s)...");
497 const std::map<int, ArLaser*> *lasers =
robot->getLaserMap();
498 ROS_INFO_NAMED(
"rosaria",
"rosaria: there are %lu connected lasers", lasers->size());
499 for(std::map<int, ArLaser*>::const_iterator i = lasers->begin(); i != lasers->end(); ++i)
501 ArLaser *l = i->second;
503 std::string tfname(
"laser");
504 if(lasers->size() > 1 || ln > 1)
507 ROS_INFO_NAMED(
"rosaria",
"rosaria: Creating publisher for laser #%d named %s with tf frame name %s", ln, l->getName(), tfname.c_str());
511 ROS_INFO_NAMED(
"rosaria",
"rosaria: Done creating laser publishers");
515 cmdvel_sub =
n.
subscribe(
"cmd_vel", 1, (boost::function <
void(
const geometry_msgs::TwistConstPtr&)>)
519 double cmdvel_timeout_param = 0.6;
520 n.
param(
"cmd_vel_timeout", cmdvel_timeout_param, 0.6);
522 if (cmdvel_timeout_param > 0.0)
542 position.twist.twist.angular.z =
robot->getRotVel()*M_PI/180;
549 ROS_DEBUG(
"RosAria: publish: (time %f) pose x: %f, pose y: %f, pose angle: %f; linear vel x: %f, vel y: %f; angular vel z: %f",
551 (double)
position.pose.pose.position.x,
552 (
double)
position.pose.pose.position.y,
553 (double)
position.pose.pose.orientation.w,
554 (
double)
position.twist.twist.linear.x,
555 (double)
position.twist.twist.linear.y,
556 (
double)
position.twist.twist.angular.z
572 int stall =
robot->getStallValue();
573 unsigned char front_bumpers = (
unsigned char)(stall >> 8);
574 unsigned char rear_bumpers = (
unsigned char)(stall);
579 std::stringstream bumper_info(std::stringstream::out);
581 for (
unsigned int i=0; i<
robot->getNumFrontBumpers(); i++)
583 bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
584 bumper_info <<
" " << (front_bumpers & (1 << (i+1)));
586 ROS_DEBUG(
"RosAria: Front bumpers:%s", bumper_info.str().c_str());
590 unsigned int numRearBumpers =
robot->getNumRearBumpers();
591 for (
unsigned int i=0; i<numRearBumpers; i++)
593 bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
594 bumper_info <<
" " << (rear_bumpers & (1 << (numRearBumpers-i)));
596 ROS_DEBUG(
"RosAria: Rear bumpers:%s", bumper_info.str().c_str());
602 std_msgs::Float64 batteryVoltage;
603 batteryVoltage.data =
robot->getRealBatteryVoltageNow();
606 if(
robot->haveStateOfCharge())
608 std_msgs::Float32 soc;
609 soc.data =
robot->getStateOfCharge()/100.0;
614 char s =
robot->getChargeState();
617 ROS_INFO(
"RosAria: publishing new recharge state %d.", s);
623 bool e =
robot->areMotorsEnabled();
626 ROS_INFO(
"RosAria: publishing new motors state %d.", e);
635 sensor_msgs::PointCloud cloud;
636 cloud.header.stamp =
position.header.stamp;
641 std::stringstream sonar_debug_info;
642 sonar_debug_info <<
"Sonar readings: ";
644 for (
int i = 0; i <
robot->getNumSonar(); i++) {
645 ArSensorReading* reading = NULL;
646 reading =
robot->getSonarReading(i);
648 ROS_WARN(
"RosAria: Did not receive a sonar reading.");
653 sonar_debug_info << reading->getRange() <<
" ";
667 geometry_msgs::Point32 p;
668 p.x = reading->getLocalX() / 1000.0;
669 p.y = reading->getLocalY() / 1000.0;
671 cloud.points.push_back(p);
679 sensor_msgs::PointCloud2 cloud2;
682 ROS_WARN(
"Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
699 ROS_INFO(
"RosAria: Enable motors request.");
701 if(
robot->isEStopPressed())
702 ROS_WARN(
"RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
703 robot->enableMotors();
711 ROS_INFO(
"RosAria: Disable motors request.");
713 robot->disableMotors();
723 ROS_INFO(
"new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z,
veltime.
toSec() );
726 robot->setVel(msg->linear.x*1e3);
727 if(
robot->hasLatVel())
728 robot->setLatVel(msg->linear.y*1e3);
729 robot->setRotVel(msg->angular.z*180/M_PI);
731 ROS_DEBUG(
"RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s",
veltime.
toSec(),
732 (double) msg->linear.x * 1e3, (
double) msg->linear.y * 1e3, (double) msg->angular.z * 180/M_PI);
742 if(
robot->hasLatVel())
743 robot->setLatVel(0.0);
744 robot->setRotVel(0.0);
750 int main(
int argc,
char** argv )
758 if( node->
Setup() != 0 )
760 ROS_FATAL(
"RosAria: ROS node setup failed... \n" );
768 ROS_INFO(
"RosAria: Quitting... \n" );
ros::Duration cmdvel_timeout
bool enable_motors_cb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
std::string frame_id_bumper
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::ServiceServer enable_srv
geometry_msgs::TransformStamped odom_trans
ros::Subscriber cmdvel_sub
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void cmdvel_cb(const geometry_msgs::TwistConstPtr &)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
std_msgs::Int8 recharge_state
nav_msgs::Odometry position
std::string aria_log_filename
std::string frame_id_base_link
ROSCPP_DECL void spin(Spinner &spinner)
Node that interfaces between ROS and mobile robot base features via ARIA library. ...
std_msgs::Bool motors_state
bool publish_sonar_pointcloud2
RosAriaNode(ros::NodeHandle n)
static Quaternion createQuaternionFromYaw(double yaw)
void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
ros::Publisher voltage_pub
bool published_motors_state
std::string frame_id_odom
ArLaserConnector * laserConnector
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
rosaria::BumperState bumpers
ros::Publisher sonar_pointcloud2_pub
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define ROS_DEBUG_STREAM(args)
int main(int argc, char **argv)
ros::ServiceServer disable_srv
uint32_t getNumSubscribers() const
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
static void poseTFToMsg(const Pose &bt, geometry_msgs::Pose &msg)
static bool convertPointCloudToPointCloud2(const sensor_msgs::PointCloud &input, sensor_msgs::PointCloud2 &output)
ros::Timer cmdvel_watchdog_timer
ros::Publisher state_of_charge_pub
ArFunctorC< RosAriaNode > myPublishCB
bool disable_motors_cb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
ros::Publisher recharge_state_pub
bool getParam(const std::string &key, std::string &s) const
std::string frame_id_sonar
ros::Publisher bumpers_pub
#define ROS_FATAL_NAMED(name,...)
void cmdvel_watchdog(const ros::TimerEvent &event)
void sonarConnectCb()
Called when another node subscribes or unsubscribes from sonar topic.
ros::Publisher motors_state_pub
dynamic_reconfigure::Server< rosaria::RosAriaConfig > * dynamic_reconfigure_server
tf::TransformBroadcaster odom_broadcaster