编写server.cpp让小乌龟以一定速度旋转一定角度
在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
结语
刚开始尝试自己写一点代码，感觉写的比较乱，有问题还望大家指正~