【ROS】使用Pioneer3at 声呐避障

LZ之前准备使用kinect转换成laser信息,然后对障碍物进行避障.rgbd传感器还是存在限制的,使用kinect的视野宽度大约只有57度,RGB-D摄像头对50厘米(大约2英寸)以内的物体是无法看见的.

所以,如果障碍物距离小车很近,或者不在视野的角度之内,是很容易撞上障碍物的(LZ血泪的教训,因为实验室空间有限,所以…)

如果使用雷达的话还是增加成本的问题,虽然有,但是不想添加…

最后使用的就是pioneer3at中声呐,其中一共有16个声呐.LZ只用到前八个,具体的手册如下:

当中还涉及到对ARIA库的使用,建议看LZ的这篇博客之前先简单翻阅一下关于ARIA API部分,熟悉一下基本的接口使用方法,阅读一下rosaria的源码.

最后附上源码
用于连接pioneer3at和sonar的头文件connect_pioneer.h

// // Created by felaim on 5/31/18. // //Include files #include <stdio.h> #include <math.h> #ifdef ADEPT_PKG #include <Aria.h> #else #include<Aria/Aria.h> #endif #include "ros/ros.h" #include "geometry_msgs/Twist.h" #include "geometry_msgs/Pose.h" #include "geometry_msgs/PoseStamped.h" #include <sensor_msgs/PointCloud.h> // 针对的是声呐产生的数据 #include <sensor_msgs/PointCloud2.h> #include <sensor_msgs/point_cloud_conversion.h> // 可以将声呐数据转换成点云的形式 #include "nav_msgs/Odometry.h" #include "rosaria/BumperState.h" #include "tf/tf.h" #include "tf/transform_listener.h" #include "tf/transform_broadcaster.h" #include "tf/transform_datatypes.h" #include <dynamic_reconfigure/server.h> #include <rosaria/RosAriaConfig.h> #include "std_msgs/Float64.h" #include "std_msgs/Float32.h" #include "std_msgs/Int8.h" #include "std_msgs/Bool.h" #include "std_srvs/Empty.h" #include <sstream> class RosAriaNode { public: RosAriaNode(ros::NodeHandle n); virtual ~RosAriaNode(); public: int Setup(); void cmdvel_cb(const geometry_msgs::TwistConstPtr &); void cmdvel_watchdog(const ros::TimerEvent &event); void spin(); void publish(); void sonarConnectCb(); void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level); void readParameters(); void robotWander(); protected: ros::NodeHandle n; ros::Publisher pose_pub; ros::Publisher bumpers_pub; ros::Publisher sonar_pub; ros::Publisher sonar_pointcloud2_pub; ros::Publisher voltage_pub; ros::Publisher recharge_state_pub; std_msgs::Int8 recharge_state; ros::Publisher state_of_charge_pub; ros::Publisher motors_state_pub; std_msgs::Bool motors_state; bool published_motors_state; ros::Subscriber cmdvel_sub; ros::ServiceServer enable_srv; ros::ServiceServer disable_srv; bool enable_motors_cb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response); bool disable_motors_cb(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response); ros::Time veltime; ros::Timer cmdvel_watchdog_timer; ros::Duration cmdvel_timeout; std::string serial_port; int serial_baud; ArRobotConnector *conn; // ArLaserConnector *laserConnector; ArRobot *robot; nav_msgs::Odometry position; rosaria::BumperState bumpers; ArPose pos; ArFunctorC<RosAriaNode> myPublishCB; //ArRobot::ChargeState batteryCharge; //for odom->base_link transform tf::TransformBroadcaster odom_broadcaster; geometry_msgs::TransformStamped odom_trans; std::string frame_id_odom; std::string frame_id_base_link; std::string frame_id_bumper; std::string frame_id_sonar; // flag indicating whether sonar was enabled or disabled on the robot bool sonar_enabled; // enable and publish sonar topics. set to true when first subscriber connects, set to false when last subscriber disconnects. bool publish_sonar; bool publish_sonar_pointcloud2; // Debug Aria bool debug_aria; std::string aria_log_filename; // Robot Calibration Parameters (see readParameters() function) 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). // dynamic_reconfigure dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server; // whether to publish aria lasers // bool publish_aria_lasers; }; //这里读取的参数主要就是之前进行线速度\角速度校正给的参数,因为我拿到的小车都没气了, // 后来自己充气后,胎压可能不一致,要进行校正,具体的参数可以写在launch文件中 void RosAriaNode::readParameters() { // Robot Parameters. If a parameter was given and is nonzero, set it now. // Otherwise, get default value for this robot (from getOrigRobotConfig()). // Parameter values are stored in member variables for possible later use by the user with dynamic reconfigure. robot->lock(); ros::NodeHandle n_("~"); if (n_.getParam("TicksMM", TicksMM) && TicksMM > 0) { ROS_INFO("Setting robot TicksMM from ROS Parameter: %d", TicksMM); robot->comInt(93, TicksMM); } else { TicksMM = robot->getOrigRobotConfig()->getTicksMM(); ROS_INFO("This robot's TicksMM parameter: %d", TicksMM); //n_.setParam( "TicksMM", TicksMM); } if (n_.getParam("DriftFactor", DriftFactor) && DriftFactor != -99999) { ROS_INFO("Setting robot DriftFactor from ROS Parameter: %d", DriftFactor); robot->comInt(89, DriftFactor); } else { DriftFactor = robot->getOrigRobotConfig()->getDriftFactor(); ROS_INFO("This robot's DriftFactor parameter: %d", DriftFactor); //n_.setParam( "DriftFactor", DriftFactor); } if (n_.getParam("RevCount", RevCount) && RevCount > 0) { ROS_INFO("Setting robot RevCount from ROS Parameter: %d", RevCount); robot->comInt(88, RevCount); } else { RevCount = robot->getOrigRobotConfig()->getRevCount(); ROS_INFO("This robot's RevCount parameter: %d", RevCount); //n_.setParam( "RevCount", RevCount); } robot->unlock(); } void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level) { // // Odometry Settings // //先对robot进行lock,在进行修改 robot->lock(); if (TicksMM != config.TicksMM && config.TicksMM > 0) { ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM); TicksMM = config.TicksMM; robot->comInt(93, TicksMM); } if (DriftFactor != config.DriftFactor && config.DriftFactor != -99999) { ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor); DriftFactor = config.DriftFactor; robot->comInt(89, DriftFactor); } if (RevCount != config.RevCount && config.RevCount > 0) { ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount); RevCount = config.RevCount; robot->comInt(88, RevCount); } // // Acceleration Parameters // int value; value = config.trans_accel * 1000; if (value != robot->getTransAccel() && value > 0) { ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %d", value); robot->setTransAccel(value); } value = config.trans_decel * 1000; if (value != robot->getTransDecel() && value > 0) { ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value); robot->setTransDecel(value); } value = config.lat_accel * 1000; if (value != robot->getLatAccel() && value > 0) { ROS_INFO("Setting LatAccel from Dynamic Reconfigure: %d", value); if (robot->getAbsoluteMaxLatAccel() > 0) robot->setLatAccel(value); } value = config.lat_decel * 1000; if (value != robot->getLatDecel() && value > 0) { ROS_INFO("Setting LatDecel from Dynamic Reconfigure: %d", value); if (robot->getAbsoluteMaxLatDecel() > 0) robot->setLatDecel(value); } value = config.rot_accel * 180 / M_PI; if (value != robot->getRotAccel() && value > 0) { ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %d", value); robot->setRotAccel(value); } value = config.rot_decel * 180 / M_PI; if (value != robot->getRotDecel() && value > 0) { ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %d", value); robot->setRotDecel(value); } //修改结束后,对robot进行unlock robot->unlock(); } /// Called when another node subscribes or unsubscribes from sonar topic. void RosAriaNode::sonarConnectCb() { publish_sonar = (sonar_pub.getNumSubscribers() > 0); publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0); robot->lock(); if (publish_sonar || publish_sonar_pointcloud2) { robot->enableSonar(); sonar_enabled = false; } else if (!publish_sonar && !publish_sonar_pointcloud2) { robot->disableSonar(); sonar_enabled = true; } robot->unlock(); } RosAriaNode::RosAriaNode(ros::NodeHandle nh) : n(nh), serial_port(""), serial_baud(0), conn(NULL), robot(NULL), myPublishCB(this, &RosAriaNode::publish), sonar_enabled(false), publish_sonar(false), publish_sonar_pointcloud2(false), debug_aria(false), TicksMM(-1), DriftFactor(-99999), RevCount(-1) { // read in runtime parameters // port and baud n.param("port", serial_port, std::string("/dev/ttyUSB0")); ROS_INFO("RosAria: set port: [%s]", serial_port.c_str()); n.param("baud", serial_baud, 0); if (serial_baud != 0) ROS_INFO("RosAria: set serial port baud rate %d", serial_baud); // handle debugging more elegantly n.param("debug_aria", debug_aria, false); // default not to debug n.param("aria_log_filename", aria_log_filename, std::string("Aria.log")); // whether to connect to lasers using aria // n.param("publish_aria_lasers", publish_aria_lasers, false); // Get frame_ids to use. n.param("odom_frame", frame_id_odom, std::string("odom")); n.param("base_link_frame", frame_id_base_link, std::string("base_link")); n.param("bumpers_frame", frame_id_bumper, std::string("bumpers")); n.param("sonar_frame", frame_id_sonar, std::string("sonar")); // advertise services for data topics // second argument to advertise() is queue size. // other argmuments (optional) are callbacks, or a boolean "latch" flag (whether to send current data to new // subscribers when they subscribe). // See ros::NodeHandle API docs. //以第一行发布信息为例,后面几行以此类推 //告诉master我们将要在chatter(话题名,这里指pose)上发布nav_msgs::Odometry消息类型的信息 //这样master就会告诉所有订阅了chatter的节点,将要有数据要发布 //第二个参数是发布序列的大小,如果我们发布的信息的频率太郎高,缓冲区的消息在大于1000个的时候就会开始丢弃先前发布的消息 pose_pub = n.advertise<nav_msgs::Odometry>("pose", 1000); bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state", 1000); sonar_pub = n.advertise<sensor_msgs::PointCloud>("sonar", 50, boost::bind(&RosAriaNode::sonarConnectCb, this), boost::bind(&RosAriaNode::sonarConnectCb, this)); sonar_pointcloud2_pub = n.advertise<sensor_msgs::PointCloud2>("sonar_pointcloud2", 50, boost::bind(&RosAriaNode::sonarConnectCb, this), boost::bind(&RosAriaNode::sonarConnectCb, this)); voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000); recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ ); recharge_state.data = -2; state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100); motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ ); motors_state.data = false; published_motors_state = false; // advertise enable/disable services enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this); disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this); veltime = ros::Time::now(); } //RosAriaNode的析构函数,断开电机,声呐,机器人停止运动,关闭Aria库 RosAriaNode::~RosAriaNode() { // disable motors and sonar. robot->disableMotors(); robot->disableSonar(); robot->stopRunning(); // 程序结束后等待用户的ctrl-C命令 robot->waitForRunExit(); Aria::shutdown(); } int RosAriaNode::Setup() { // Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be // called once per instance, and these objects need to persist until the process terminates. robot = new ArRobot(); ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx) // Now add any parameters given via ros params (see RosAriaNode constructor): // if serial port parameter contains a ':' character, then interpret it as hostname:tcpport // for wireless serial connection. Otherwise, interpret it as a serial port name. size_t colon_pos = serial_port.find(":"); if (colon_pos != std::string::npos) { args->add("-remoteHost"); // pass robot's hostname/IP address to Aria args->add(serial_port.substr(0, colon_pos).c_str()); args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria args->add(serial_port.substr(colon_pos+1).c_str()); } else { args->add("-robotPort %s", serial_port.c_str()); // pass robot's serial port to Aria } // if a baud rate was specified in baud parameter if(serial_baud != 0) { args->add("-robotBaud %d", serial_baud); } if( debug_aria ) { // turn on all ARIA debugging args->add("-robotLogPacketsReceived"); // log received packets args->add("-robotLogPacketsSent"); // log sent packets args->add("-robotLogVelocitiesReceived"); // log received velocities args->add("-robotLogMovementSent"); args->add("-robotLogMovementReceived"); ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true); } // Connect to the robot conn = new ArRobotConnector(argparser, robot); // warning never freed if (!conn->connectRobot()) { ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)"); return 1; } // if(publish_aria_lasers) // laserConnector = new ArLaserConnector(argparser, robot, conn); // causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params if(!Aria::parseArgs()) { ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!"); return 1; } readParameters(); // Start dynamic_reconfigure server dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>; // Setup Parameter Minimums and maximums rosaria::RosAriaConfig dynConf_min; rosaria::RosAriaConfig dynConf_max; dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000; dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000; // TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals // Until then, set unit length interval. dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000; dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000; dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180; dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180; dynConf_min.trans_accel = 0; dynConf_min.trans_decel = 0; dynConf_min.lat_accel = 0; dynConf_min.lat_decel = 0; dynConf_min.rot_accel = 0; dynConf_min.rot_decel = 0; dynConf_min.TicksMM = 0; dynConf_max.TicksMM = 200; dynConf_min.DriftFactor = -99999; dynConf_max.DriftFactor = 32767; dynConf_min.RevCount = 0; dynConf_max.RevCount = 65535; dynamic_reconfigure_server->setConfigMax(dynConf_max); dynamic_reconfigure_server->setConfigMin(dynConf_min); rosaria::RosAriaConfig dynConf_default; dynConf_default.trans_accel = robot->getTransAccel() / 1000; dynConf_default.trans_decel = robot->getTransDecel() / 1000; dynConf_default.lat_accel = robot->getLatAccel() / 1000; dynConf_default.lat_decel = robot->getLatDecel() / 1000; dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180; dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180; dynConf_default.TicksMM = 0; dynConf_default.DriftFactor = -99999; dynConf_default.RevCount = 0; dynamic_reconfigure_server->setConfigDefault(dynConf_default); dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2)); // Enable the motors robot->enableMotors(); // disable sonars on startup // robot->disableSonar(); // enable sonars on startup robot->enableSonar(); // callback will be called by ArRobot background processing thread for every SIP data packet received from robot robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB); // Initialize bumpers with robot number of bumpers bumpers.front_bumpers.resize(robot->getNumFrontBumpers()); bumpers.rear_bumpers.resize(robot->getNumRearBumpers()); // Run ArRobot background processing thread robot->runAsync(true); // connect to lasers and create publishers // if(publish_aria_lasers) // { // ROS_INFO_NAMED("rosaria", "rosaria: Connecting to laser(s) configured in ARIA parameter file(s)..."); // if (!laserConnector->connectLasers()) // { // ROS_FATAL_NAMED("rosaria", "rosaria: Error connecting to laser(s)..."); // return 1; // } // // robot->lock(); // const std::map<int, ArLaser*> *lasers = robot->getLaserMap(); // ROS_INFO_NAMED("rosaria", "rosaria: there are %lu connected lasers", lasers->size()); // for(std::map<int, ArLaser*>::const_iterator i = lasers->begin(); i != lasers->end(); ++i) // { // ArLaser *l = i->second; // int ln = i->first; // std::string tfname("laser"); // if(lasers->size() > 1 || ln > 1) // no number if only one laser which is also laser 1 // tfname += ln; // tfname += "_frame"; // ROS_INFO_NAMED("rosaria", "rosaria: Creating publisher for laser #%d named %s with tf frame name %s", ln, l->getName(), tfname.c_str()); // new LaserPublisher(l, n, true, tfname); // } // robot->unlock(); // ROS_INFO_NAMED("rosaria", "rosaria: Done creating laser publishers"); // } // subscribe to command topics cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>) boost::bind(&RosAriaNode::cmdvel_cb, this, _1 )); // register a watchdog for cmd_vel timeout double cmdvel_timeout_param = 0.6; n.param("cmd_vel_timeout", cmdvel_timeout_param, 0.6); cmdvel_timeout = ros::Duration(cmdvel_timeout_param); if (cmdvel_timeout_param > 0.0) cmdvel_watchdog_timer = n.createTimer(ros::Duration(0.1), &RosAriaNode::cmdvel_watchdog, this); ROS_INFO_NAMED("rosaria", "rosaria: Setup complete"); return 0; } //ros::spin() 在调用后不会再返回，也就是你的主程序到这儿就不往下执行了， // 而 ros::spinOnce() 后者在调用后还可以继续执行之后的程序。 void RosAriaNode::spin() { ros::spin(); } void RosAriaNode::publish() { // Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked. pos = robot->getPose(); tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000, pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm. position.twist.twist.linear.x = robot->getVel()/1000.0; //Aria returns velocity in mm/s. position.twist.twist.linear.y = robot->getLatVel()/1000.0; position.twist.twist.angular.z = robot->getRotVel()*M_PI/180; position.header.frame_id = frame_id_odom; position.child_frame_id = frame_id_base_link; position.header.stamp = ros::Time::now(); pose_pub.publish(position); 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", position.header.stamp.toSec(), (double)position.pose.pose.position.x, (double)position.pose.pose.position.y, (double)position.pose.pose.orientation.w, (double)position.twist.twist.linear.x, (double)position.twist.twist.linear.y, (double)position.twist.twist.angular.z ); // publishing transform odom->base_link odom_trans.header.stamp = ros::Time::now(); odom_trans.header.frame_id = frame_id_odom; odom_trans.child_frame_id = frame_id_base_link; odom_trans.transform.translation.x = pos.getX()/1000; odom_trans.transform.translation.y = pos.getY()/1000; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180); odom_broadcaster.sendTransform(odom_trans); // getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers) int stall = robot->getStallValue(); unsigned char front_bumpers = (unsigned char)(stall >> 8); unsigned char rear_bumpers = (unsigned char)(stall); bumpers.header.frame_id = frame_id_bumper; bumpers.header.stamp = ros::Time::now(); std::stringstream bumper_info(std::stringstream::out); // Bit 0 is for stall, next bits are for bumpers (leftmost is LSB) for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++) { bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1; bumper_info << " " << (front_bumpers & (1 << (i+1))); } ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str()); bumper_info.str(""); // Rear bumpers have reverse order (rightmost is LSB) unsigned int numRearBumpers = robot->getNumRearBumpers(); for (unsigned int i=0; i<numRearBumpers; i++) { bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1; bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i))); } ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str()); bumpers_pub.publish(bumpers); //Publish battery information // TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option std_msgs::Float64 batteryVoltage; batteryVoltage.data = robot->getRealBatteryVoltageNow(); voltage_pub.publish(batteryVoltage); if(robot->haveStateOfCharge()) { std_msgs::Float32 soc; soc.data = robot->getStateOfCharge()/100.0; state_of_charge_pub.publish(soc); } // publish recharge state if changed char s = robot->getChargeState(); if(s != recharge_state.data) { ROS_INFO("RosAria: publishing new recharge state %d.", s); recharge_state.data = s; recharge_state_pub.publish(recharge_state); } // publish motors state if changed bool e = robot->areMotorsEnabled(); if(e != motors_state.data || !published_motors_state) { ROS_INFO("RosAria: publishing new motors state %d.", e); motors_state.data = e; motors_state_pub.publish(motors_state); published_motors_state = true; } // Publish sonar information, if enabled. if (publish_sonar || publish_sonar_pointcloud2) { sensor_msgs::PointCloud cloud; //sonar readings. cloud.header.stamp = position.header.stamp; //copy time. // sonar sensors relative to base_link cloud.header.frame_id = frame_id_sonar; std::stringstream sonar_debug_info; // Log debugging info sonar_debug_info << "Sonar readings: "; for (int i = 0; i < robot->getNumSonar(); i++) { ArSensorReading* reading = NULL; reading = robot->getSonarReading(i); if(!reading) { ROS_WARN("RosAria: Did not receive a sonar reading."); continue; } // getRange() will return an integer between 0 and 5000 (5m) sonar_debug_info << reading->getRange() << " "; // local (x,y). Appears to be from the centre of the robot, since values may // exceed 5000. This is good, since it means we only need 1 transform. // x & y seem to be swapped though, i.e. if the robot is driving north // x is north/south and y is east/west. // //ArPose sensor = reading->getSensorPosition(); //position of sensor. // sonar_debug_info << "(" << reading->getLocalX() // << ", " << reading->getLocalY() // << ") from (" << sensor.getX() << ", " // << sensor.getY() << ") ;; " ; //add sonar readings (robot-local coordinate frame) to cloud geometry_msgs::Point32 p; p.x = reading->getLocalX() / 1000.0; p.y = reading->getLocalY() / 1000.0; p.z = 0.0; cloud.points.push_back(p); } ROS_DEBUG_STREAM(sonar_debug_info.str()); // publish topic(s) if(publish_sonar_pointcloud2) { sensor_msgs::PointCloud2 cloud2; if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2)) { ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time."); } else { sonar_pointcloud2_pub.publish(cloud2); } } if(publish_sonar) { sonar_pub.publish(cloud); } } // end if sonar_enabled } bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { ROS_INFO("RosAria: Enable motors request."); robot->lock(); if(robot->isEStopPressed()) ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable."); robot->enableMotors(); robot->unlock(); // todo could wait and see if motors do become enabled, and send a response with an error flag if not return true; } bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) { ROS_INFO("RosAria: Disable motors request."); robot->lock(); robot->disableMotors(); robot->unlock(); // todo could wait and see if motors do become disabled, and send a response with an error flag if not return true; } void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg) { veltime = ros::Time::now(); ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() ); robot->lock(); robot->setVel(msg->linear.x*1e3); if(robot->hasLatVel()) robot->setLatVel(msg->linear.y*1e3); robot->setRotVel(msg->angular.z*180/M_PI); robot->unlock(); 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(), (double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI); } void RosAriaNode::cmdvel_watchdog(const ros::TimerEvent& event) { // stop robot if no cmd_vel message was received for 0.6 seconds if (ros::Time::now() - veltime > ros::Duration(0.6)) { robot->lock(); robot->setVel(0.0); if(robot->hasLatVel()) robot->setLatVel(0.0); robot->setRotVel(0.0); robot->unlock(); } } void RosAriaNode::robotWander() { robot->lock(); ArKeyHandler keyHandler; ArSonarDevice sonar; Aria::setKeyHandler(&keyHandler); robot->attachKeyHandler(&keyHandler); //puts("This program will make the robot wander around. It uses some avoidance\n" //"actions if obstacles are detected, otherwise it just has a\n" //"constant forward velocity.\n\nPress Escape to exit."); ArActionStallRecover recover; //ArActionBumpers bumpers; ArActionAvoidFront avoidFrontNear("Avoid Front Near",500, 0); ArActionAvoidFront avoidFrontFar; ArActionConstantVelocity constantVelocity("Constant Velocity",150); ArActionLimiterForwards limiter("speed limiter near", 300, 600, 250); // limiter for far away obstacles //ArActionLimiterForwards (const char *name="speed limiter", double stopDistance=250, double slowDistance=1000, double slowSpeed=200, double widthRatio=1) ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 200); robot->addRangeDevice(&sonar); robot->addAction(&limiter, 95); robot->addAction(&limiterFar, 90); robot->addAction(&recover, 100); //robot->addAction(&bumpers, 75); robot->addAction(&avoidFrontNear, 50); robot->addAction(&avoidFrontFar, 49); robot->addAction(&constantVelocity, 25); robot->enableMotors(); //it is really important robot->enableSonar(); robot->unlock(); robot->waitForRunExit(); }

connect_pioneer.cpp

// // Created by felaim on 6/6/18. // #include "connect_pioneer.h" int main(int argc, char** argv) { ros::init(argc, argv, "felaim_2dnav"); ros::NodeHandle n(std::string("~")); Aria::init(); RosAriaNode *node = new RosAriaNode(n); if(node->Setup() != 0) { ROS_FATAL("RosAria: ROS node setup failed ...\n"); return -1; } ROS_INFO("\n The robot is wandering now!\n\n********\n\n\t\t Please press ESC to STOP! \n\n********"); node->robotWander(); delete node; ROS_INFO("RosAria: Quitting ... \n"); return 0; }

用catkin_make进行编译

用MobileSim仿真的结果

热的头晕晕的每天,LZ喜欢冬天O(∩_∩)O哈哈~脑子会比较清醒…