使用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哈哈~脑子会比较清醒…