第三章 ROS通信编程 课后作业

代码包

所有代码已上传至github网站：
github链接: https://github.com/YuemingBi/ros_practice/tree/master

课后作业

第一题

（1）vel_pose.cpp

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/Twist.h"

void callback(const turtlesim::Pose::ConstPtr& pose){
    ROS_INFO("x: %f", pose->x);
    ROS_INFO("y: %f", pose->y);
    ROS_INFO("theta: %f", pose->theta);
    ROS_INFO("linear_velocity: %f", pose->linear_velocity);
    ROS_INFO("angular_velocity: %f", pose->angular_velocity);
    ROS_INFO("---");
}

int main(int argc, char ** argv){
    ros::init(argc, argv, "pose_subscriber");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 1000);
    ros::Subscriber sub = n.subscribe("turtle1/pose", 1000, callback);
    ros::Rate loop_rate(10);

    while(ros::ok()){
        geometry_msgs::Twist msg;
        msg.linear.x = 0.5;
        msg.angular.z = 0.5;
        //ROS_INFO("vel_publish: linear.x=%.1f angular.z=%.1f", msg.linear.x, msg.angular.z);
        pub.publish(msg);
        loop_rate.sleep();
        ros::spinOnce();
    }

    return 0;
}

主要编程思想：将发布消息的函数pub.publish()和ros::spinOnce()都放在while循环里，这样就可保证发布一次消息后订阅一次消息进入回调函数，显示pose信息。

（2）运行结果

$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun test1 vel_pose

第二题

（1）spawn_client.cpp

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
#include "ctime"
#include "sstream"

int main(int argc, char **argv){
    ros::init(argc, argv, "spawn_client");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;

    int number[2];
    int count = 0;
    int input1 = 12;
    int input2 = 999999999;
    srand(time(0));
    number[0] = rand()%input1;
    number[1] = rand()%input1;
    number[2] = rand()%input1;
    count = rand()%input2;

    std::stringstream ss;
    ss << "turtle"<< count;

    srv.request.x = number[0];
    srv.request.y = number[1];
    srv.request.theta = number[2];
    srv.request.name = ss.str();
    client.call(srv);
    ROS_INFO("show result: %s", srv.response.name.c_str());

    return 0;
}

程序里面用到了随机数，随机位置、随机角度（这里对应了第三题的位置不重叠）。

（2）运行结果

$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun test2 spawn_client
...

第三题

第一问

（1）turtle_spawn.cpp

#include "ros/ros.h"
#include "turtlesim/Spawn.h"
#include "ctime"

int main(int argc, char** argv){
    ros::init(argc, argv, "turtle_spawn");
    ros::NodeHandle n;
    ros::ServiceClient client = n.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;

    int input1 = 10;
    int number[2];
    srand(time(0));
    number[0] = rand()%input1;
    number[1] = rand()%input1;
    number[2] = rand()%input1;

    srv.request.x = number[0];
    srv.request.y = number[1];
    srv.request.theta = number[2];
    srv.request.name = argv[1];
    client.call(srv);
    ROS_INFO("show result: %s", srv.response.name.c_str());

    return 0;
}

注意：程序中的argv[1]代表命令行中输入的海龟的名字。

（2）运行结果

$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun test3 turtle_spawn turtle2
$ rosrun test3 turtle_spawn turtle3
$ rosrun test3 turtle_spawn turtle4
$ rosrun test3 turtle_spawn turtle5
$ rosrun test3 turtle_spawn turtle6
$ rosrun test3 turtle_spawn turtle7

第二问

第二问可以利用多个节点来完成此功能，利用circular_motion节点来接收命令行中的信息，包括海龟名字、运动还是停止、运动的速度，然后将判断结果传入motion_control服务当中，进行海龟的控制。这样就大大简化了问题。

（1）circular_motion.cpp

#include "ros/ros.h"
#include "test3/turtlesrv.h"
#include "sstream"

int main(int argc, char **argv){
    ros::init(argc, argv, "circular_motion");
    ros::NodeHandle n;
    std::stringstream ss;
    ss << argv[1] << "/cmd_vel";
    ros::ServiceClient client = n.serviceClient<test3::turtlesrv>("motion_control");
    test3::turtlesrv srv;
    srv.request.turtlename = ss.str();
    srv.request.x = atof(argv[3]);
    srv.request.z = atof(argv[4]);
    std::string name = argv[2];

    if(name=="begin"){
        srv.request.data = true;
        ROS_INFO("Start moving");
    }
    else if(name=="stop"){
        srv.request.data = false;
        ROS_INFO("Stop moving");
    }

    if(client.call(srv)){
        ROS_INFO("Start moving");
    }
    else{
        ROS_INFO("Stop moving");
    }

    return 0;
}

argv[1]为海龟名字，argv[2]为运动状态（begin或stop），argv[3]为线速度，argv[4]为角速度。

（2）motion_server.cpp

#include "ros/ros.h"
#include "test3/turtlesrv.h"
#include "geometry_msgs/Twist.h"

bool control(test3::turtlesrv::Request &req, test3::turtlesrv::Response &res){
    if(req.data){
        ros::NodeHandle n;
        ros::Publisher pub = n.advertise<geometry_msgs::Twist>(req.turtlename, 1000);
        ros::Rate loop_rate(10);
        while(ros::ok()){
            geometry_msgs::Twist msg;
            msg.linear.x = req.x;
            msg.angular.z = req.z;
            ROS_INFO("vel_publish: linear.x=%.1f angular.z=%.1f", msg.linear.x, msg.angular.z);
            pub.publish(msg);
            loop_rate.sleep();
            ros::spinOnce();
        }

        return true;
    }
    else{
        ros::NodeHandle n;
        ros::Publisher pub = n.advertise<geometry_msgs::Twist>(req.turtlename, 1000);
        ros::Rate loop_rate(10);
        while(ros::ok()){
            geometry_msgs::Twist msg;
            msg.linear.x = 0;
            msg.angular.z = 0;
            ROS_INFO("vel_publish: linear.x=%.1f angular.z=%.1f", msg.linear.x, msg.angular.z);
            pub.publish(msg);
            loop_rate.sleep();
            ros::spinOnce();
        }
    }

      return false;
}

int main(int argc, char **argv){
    ros::init(argc, argv, "motion_server");
    ros::NodeHandle n;
    ros::ServiceServer service = n.advertiseService("motion_control", control);
    ros::spin();

    return 0;
}

从程序当中可以看出，如果motion_server接收的消息为True（即circular_motion接收到命令行的消息为begin），则会发布运动指令。同理如果motion_server接收的消息为False（即circular_motion接收到命令行的消息为stop），则会发布停止指令。其中的turtlesrv.h如下。

bool data
string turtlename
float64 x
float64 z
---
bool success

（3）运行结果

指定运动海龟为turtle5，begin（开始运动），线速度和角速度分别为0.5和0.5。

$ roscore
$ rosrun turtlesim turtlesim_node
$ rosrun test3 turtle_spawn turtle2
$ rosrun test3 turtle_spawn turtle3
$ rosrun test3 turtle_spawn turtle4
$ rosrun test3 turtle_spawn turtle5
$ rosrun test3 turtle_spawn turtle6
$ rosrun test3 turtle_spawn turtle7
$ rosrun test3 motion_server
$ rosrun test3 circular_motion turtle5 begin 0.5 0.5

stop（停止运动）

$ rosrun test3 circular_motion turtle5 stop 0 0