ROS初学者编写小乌龟以一定速度旋转一定角度的server

在src文件夹下新建srv文件夹

添加rotate_angle.srv文件

float64 angle //输入的弧度
float64 angular_velocity//输入的角速度（rad/s）
float64 time//设定超时时间，超过这个时间退出循环

---
bool success//返回一个bool型的参数
string message//返回信息

编译.srv文件后，会在工作空间/devel/include中响应的包内产生相应的头文件

在src文件夹下新建server.cpp文件

//该例程将执行/rotate_angle服务，服务数据类型为service_demo/rotate_angle

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>
#include <service_demo/rotate_angle.h>
#include <cmath>

ros::Publisher turtle_angular_velocity_pub; //定义角速度的发布者
ros::Subscriber turtle_angle_sub;//定义小乌龟角度的接收者，接收的话题为/turtle1/pose

geometry_msgs::Twist vel_msg;//定义type为geometry_msgs::Twist的vel_msg，用来接收传入的速度信息(弧度每秒）
turtlesim::Pose nowps,addps;//定义type为turtlesim::Pose的nowps，addps，用来表示现在的弧度，以及用来接收增加的弧度
bool pubCommand = false;
float time_out; //定义一个time_out来接收设定超时时间大小

//service 回调函数，输入参数req，输出参数res
bool angleCallback(service_demo::rotate_angle::Request &req, 
                                         service_demo::rotate_angle::Response &res)
{
    pubCommand = !pubCommand; //取反

    vel_msg.angular.z = req.angular_velocity; //传入的角速度赋值给前面定义的vel_msg，.angular.z表示为绕z轴旋转的角速度

    addps.theta = req.angle;//传入的角度赋值给前面定义的addps，.theta表示角度

    time_out = req.time;//传入的时间赋值给前面定义的time_out
    //至此可以发现一共接收三个信息：旋转的角速度，旋转的弧度，以及超时时间（超过这个时间退出循环）

    //显示请求数据
    ROS_INFO("Publish angle [%f] , angle_velocity [%f] ",req.angle,req.angular_velocity);

    //显示现在时间
    ros::Time begin =ros::Time::now();
    ROS_INFO_STREAM("The beginning of time :"<<begin);

    //设置反馈数据
    res.success = true;
    res.message = "Change state!";

    return true;

}
//subscriber回调函数
void poseCallback(const turtlesim::Pose pose)
{
    //小乌龟现在的位姿theta赋值给前面定义的nowps
    nowps.theta = pose.theta;
}

//主函数
int main(int argc ,char **argv)
{
   //ROS节点初始化 
    ros::init(argc,argv,"rotate_angle_server");

    ros::NodeHandle n;//创建节点句柄

    //创建一个名为/rotate_angle的server，注册回调函数angleCallback
    ros::ServiceServer angle_service = n.advertiseService("/rotate_angle",angleCallback);

    //创建一个Publisher，发布名为/turtle1/cmd_vel的topic ，消息类型为geometry_msgs::Twist，队列长度为10
    turtle_angular_velocity_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);

    //创建一个Subscriber，接收名为/turtle1/pose的topic,消息类型为tuetlesim/Pose,队列长度为1000
    turtle_angle_sub = n.subscribe<turtlesim::Pose>("/turtle1/pose",1000,poseCallback);

    int count = 0;
    
    turtlesim::Pose firstps,finalps;//定义初始时刻小乌龟的位姿firstps，终止时刻的位姿finalps

    ROS_INFO("Ready to receive command");//循环等待回调函数

    ros::Rate loop_rate(10);//设置循环频率

    //ros::spin();

    while(ros::ok())
   {
       //查看一次回调函数队列
        ros::spinOnce();

        if(pubCommand)
        {
            if(count == 0)
            {
                firstps.theta = nowps.theta;//记录初始时刻的theta
                ROS_INFO_STREAM("The first pose theta is "<<firstps.theta); 
            }
            if(fabs(nowps.theta-firstps.theta) <= 3.1415926) //因为是弧度制，变化范围为-3.1415~+3.1415，所以需要分情况来进行讨论
            {
                if(fabs(nowps.theta-firstps.theta)<addps.theta)//当现在位置与初始位置的绝对值小于用户要求旋转的角度
                {
                turtle_angular_velocity_pub.publish(vel_msg);//发布前面定义的vel_msg，其中绕z轴的速度是用户自行设定然后赋值的
                count++;
                if(count > 11*time_out)//因为频率为10，所以一秒钟执行10次循环，用户输入时间*10也就是最大循环次数，当超过当前这个次数时也就可以视为超时，这里我们取的大一些，乘上11，超时后直接退出循环
                {
                    ROS_INFO("False");
                    break;
                }
                }
                else
                {
                    finalps.theta=nowps.theta;//如果没有超过用户设定的时间，旋转完成最后输出最终的theta
                    ROS_INFO_STREAM("The first pose theta "<<firstps.theta<<"; The final pose theta "<<finalps.theta); 
                    break;
                     //ros::shutdown();
                }
            }
            if(fabs(nowps.theta-firstps.theta) > 3.1415926)
            {
                if(6.2831852-fabs(nowps.theta-firstps.theta)<addps.theta)
                {
                turtle_angular_velocity_pub.publish(vel_msg);//同上
                count++;
                if(count > 11*time_out)
                {
                    ROS_INFO("False");
                    break;
                }
                }
                else
                {
                    finalps.theta=nowps.theta;
                    ROS_INFO_STREAM("The first pose theta "<<firstps.theta<<"; The final pose theta "<<finalps.theta); 
                    break;
                    //ros::shutdown();
                }
            }
            
        }
        //按照循环频率延时
        loop_rate.sleep();
   }

    ros::Time end = ros::Time::now(); //输出结束的时间
    ROS_INFO_STREAM("The end of time :"<<end); 


    return 0;

}

运行server

rosservice call /rotate_angle 双击tab键补全，输入角速度为0.25，旋转弧度为1.0，要求时间为2.0秒，显然旋转这1弧度需要4秒，而我们设定了最大时间2秒，乌龟还没有旋转完成就返回False。

修改时间，把要求时间改为5.0秒，可以看到没有返回false，最后输出了 final pose theta

结语

刚开始尝试自己写一点代码，感觉写的比较乱，有问题还望大家指正~