ROS beginners write the server that the little turtle rotates a certain angle at a certain speed

stay src New under folder srv Folder

add to rotate_angle.srv file

float64 angle // Radians entered 
float64 angular_velocity// Input angular velocity （rad/s）
float64 time// Set the timeout , Exit the loop after this time 

---
bool success// Return to one bool The parameters of type 
string message// Return information 

compile .srv After the document , In the workspace /devel/include The corresponding header file is generated in the response package in

stay src New under folder server.cpp file

// The routine will execute /rotate_angle service , Service data type is 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; // Defines the publisher of the angular velocity 
ros::Subscriber turtle_angle_sub;// Define the receiver of the turtle's angle , The topic received is /turtle1/pose

geometry_msgs::Twist vel_msg;// Definition type by geometry_msgs::Twist Of vel_msg, Used to receive incoming speed information ( Radians per second ）
turtlesim::Pose nowps,addps;// Definition type by turtlesim::Pose Of nowps,addps, Used to express the current radian , And to receive the increased radian 
bool pubCommand = false;
float time_out; // Define a time_out To receive the set timeout size 

//service  Callback function , Input parameters req, Output parameters res
bool angleCallback(service_demo::rotate_angle::Request &req, 
                                         service_demo::rotate_angle::Response &res)
{
    
    pubCommand = !pubCommand; // Take the opposite 

    vel_msg.angular.z = req.angular_velocity; // The incoming angular velocity is assigned to the previously defined vel_msg,.angular.z Expressed as winding z Angular velocity of shaft rotation 

    addps.theta = req.angle;// The incoming angle is assigned to the previously defined addps,.theta Show angle 

    time_out = req.time;// The incoming time is assigned to the previously defined time_out
    // So far, it can be found that three messages are received ： Angular velocity of rotation , Arc of rotation , And the timeout （ Exit the loop after this time ）

    // Display request data 
    ROS_INFO("Publish angle [%f] , angle_velocity [%f] ",req.angle,req.angular_velocity);

    // Display the current time 
    ros::Time begin =ros::Time::now();
    ROS_INFO_STREAM("The beginning of time :"<<begin);

    // Set feedback data 
    res.success = true;
    res.message = "Change state!";

    return true;

}
//subscriber Callback function 
void poseCallback(const turtlesim::Pose pose)
{
    
    // The little turtle's present position theta Assigned to the previously defined nowps
    nowps.theta = pose.theta;
}

// The main function 
int main(int argc ,char **argv)
{
    
   //ROS Node initialization  
    ros::init(argc,argv,"rotate_angle_server");

    ros::NodeHandle n;// Create node handle 

    // Create a file called /rotate_angle Of server, Register callback function angleCallback
    ros::ServiceServer angle_service = n.advertiseService("/rotate_angle",angleCallback);

    // Create a Publisher, The release is called /turtle1/cmd_vel Of topic , The message type is geometry_msgs::Twist, The queue length is 10
    turtle_angular_velocity_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel",10);

    // Create a Subscriber, The receiving name is /turtle1/pose Of topic, The message type is tuetlesim/Pose, The queue length is 1000
    turtle_angle_sub = n.subscribe<turtlesim::Pose>("/turtle1/pose",1000,poseCallback);

    int count = 0;
    
    turtlesim::Pose firstps,finalps;// Define the position and posture of the little turtle at the initial time firstps, The position and posture at the end time finalps

    ROS_INFO("Ready to receive command");// Loop waiting for callback function 

    ros::Rate loop_rate(10);// Set the cycle frequency 

    //ros::spin();

    while(ros::ok())
   {
    
       // Check the callback function queue once 
        ros::spinOnce();

        if(pubCommand)
        {
    
            if(count == 0)
            {
    
                firstps.theta = nowps.theta;// Record the initial time theta
                ROS_INFO_STREAM("The first pose theta is "<<firstps.theta); 
            }
            if(fabs(nowps.theta-firstps.theta) <= 3.1415926) // Because it's a radian system , The range of variation is -3.1415~+3.1415, So we need to discuss it according to the situation 
            {
    
                if(fabs(nowps.theta-firstps.theta)<addps.theta)// When the absolute value between the current position and the initial position is less than the rotation angle required by the user 
                {
    
                turtle_angular_velocity_pub.publish(vel_msg);// Publish the previously defined vel_msg, Which revolves around z The speed of the axis is set and assigned by the user 
                count++;
                if(count > 11*time_out)// Because the frequency is 10, So one second execution 10 Secondary cycle , User input time *10 That is, the maximum number of cycles , When the current number of times is exceeded, it can also be regarded as timeout , Here we take a larger one , Multiply 11, Exit the loop directly after timeout 
                {
    
                    ROS_INFO("False");
                    break;
                }
                }
                else
                {
    
                    finalps.theta=nowps.theta;// If the time set by the user is not exceeded , The rotation completes the final output 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);// ditto 
                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();
                }
            }
            
        }
        // Delay according to the cycle frequency 
        loop_rate.sleep();
   }

    ros::Time end = ros::Time::now(); // Output end time 
    ROS_INFO_STREAM("The end of time :"<<end); 


    return 0;

}

function server

rosservice call /rotate_angle double-click tab Key completion , The input angular velocity is 0.25, The rotation radian is 1.0, The required time is 2.0 second , Obviously rotating this 1 Radian requires 4 second , And we set the maximum time 2 second , The tortoise returned before the rotation was completed False.

Modification time , Change the required time to 5.0 second , You can see that there is no return false, In the end, I output final pose theta

Conclusion

Just started trying to write a bit of code yourself , I feel it's written in a mess , If you have any questions, please correct them ~