一、机器人前行

1.forward.srv

请求参数为：
1.前行的距离
2.前行的速度
3.设定的时间，即机器人在给定的时间内运行完成

返回的参数：
1.bool值，是否到达指定位置
2.字符串

2.forward.cpp

//该例程执行/forward服务
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>
#include <forward_demo/forward.h>
#include <rotate_demo/rotate.h>
#include <sensor_msgs/Imu.h>
#include <cmath>
#include <string>

ros::Subscriber pose_sub;
ros::Publisher linear_velocity_pub;
nav_msgs::Odometry nowps;
geometry_msgs::Twist lin_msg,definite_lin;
float add_line,time_out;
bool pubCommand;
std::string msg;

bool forwardCallback(forward_demo::forward::Request &req,
                                              forward_demo::forward::Response &res)
{
    
    
    lin_msg.linear.x = req.linear_velocity;
    add_line = req.line;
    time_out = req.time;
    ROS_INFO("Publish line [%f] , linear_velocity [%f], time[%f] ",
                            req.line,req.linear_velocity,req.time);    
    
    ros::Time begin = ros::Time::now();
    ROS_INFO_STREAM("The beginning of time :"<<begin.sec);
    int count = 0;
    nav_msgs::Odometry firstps;
    nav_msgs::Odometry finalps; 

    if(count == 0)
    {
    
    
        firstps = nowps;
        ROS_INFO_STREAM("The first pose  X :"<<firstps.pose.pose.position.x<<"; Y :"<<firstps.pose.pose.position.y); 
    }
    ros::Rate loop_rate(10);

    while(ros::ok())
    {
    
    
        ros::spinOnce();
        double distance = sqrt(((nowps.pose.pose.position.x-firstps.pose.pose.position.x)*(nowps.pose.pose.position.x-firstps.pose.pose.position.x)
        +(nowps.pose.pose.position.y-firstps.pose.pose.position.y)*(nowps.pose.pose.position.y-firstps.pose.pose.position.y)));

        ros::Time now = ros::Time::now();
        if(now.sec - begin.sec >= time_out)
        {
    
    
            ROS_INFO_STREAM("Time out !");
            pubCommand = false;
            msg = "Time out !";
            break;
        }
        if(add_line > 0)
        {
    
    
            if(distance <(add_line-0.6)){
    
    
                linear_velocity_pub.publish(lin_msg);}
            else if(distance < add_line)
            {
    
    
                definite_lin.linear.x = lin_msg.linear.x * (add_line-distance)/0.6;
                if(definite_lin.linear.x > -0.06 && definite_lin.linear.x<0){
    
    
                    definite_lin.linear.x = -0.04;}
                if(definite_lin.linear.x < 0.06 && definite_lin.linear.x>0){
    
    
                    definite_lin.linear.x = 0.04;}
                linear_velocity_pub.publish(definite_lin);   
            }
            else
            {
    
    
                finalps.pose.pose.position.x = nowps.pose.pose.position.x;
                finalps.pose.pose.position.y = nowps.pose.pose.position.y;
                pubCommand = true;
                msg = "Change the state successfully !";
                ROS_INFO_STREAM("The first pose  X :"<<firstps.pose.pose.position.x<<"; Y :"<<firstps.pose.pose.position.y
                                                        <<" The final pose X :"<<finalps.pose.pose.position.x<<"; Y :"<<finalps.pose.pose.position.y);
                break;         
            }
        }
        else
        {
    
    
            pubCommand = false;
            msg = "Input error !";
            break;
        }
        count++;
        loop_rate.sleep();
    }
    res.success = pubCommand;
    res.message = msg;
    return true;       
}

void poseCallback(const nav_msgs::Odometry pose)
{
    
    
    nowps = pose;
}

int main(int argc ,char **argv)
{
    
    
    ros::init(argc,argv,"forward_server");
    ros::NodeHandle n;
    ros::ServiceServer forward_service = n.advertiseService("/forward",forwardCallback);
    pose_sub = n.subscribe("/odom",10,poseCallback);
    linear_velocity_pub = n.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity",10);
    ROS_INFO("Ready to receive command");
    ros::spin();
    return 0;
}

示例：pandas 是基于NumPy 的一种工具，该工具是为了解决数据分析任务而创建的。

二、机器人旋转

1.rotate.srv

2.rotate.cpp

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Quaternion.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>
#include <forward_demo/forward.h>
#include <rotate_demo/rotate.h>
#include <sensor_msgs/Imu.h>
#include <cmath>
#include <string>

ros::Publisher angular_velocity_pub;
ros::Subscriber imu_sub;
geometry_msgs::Twist ang_msg,definite_ang;
double nowangle;
float add_angle;
bool pubCommand;
std::string msg;
float time_out;

bool rotateCallback(rotate_demo::rotate::Request &req,
                                          rotate_demo::rotate::Response &res)
{
    
    
    ang_msg.angular.z = req.angular_velocity;
    add_angle = req.angle;
    time_out = req.time;
    ROS_INFO("Publish angle [%f] , angle_velocity [%f] , time[%f] ",
                            req.angle,req.angular_velocity,req.time);
    ros::Time begin =ros::Time::now();
    ROS_INFO_STREAM("The beginning of time :"<<begin);
    int count = 0;
    double firstangle,finalangle;

    if(count == 0)
    {
    
    
        firstangle = nowangle;
        ROS_INFO_STREAM("The first pose yaw :"<<firstangle); 
    }
    ros::Rate loop_rate(10);

    while(ros::ok())
    {
    
    
        ros::spinOnce();
        double angle_fabs = fabs(nowangle-firstangle);
        
        ros::Time now = ros::Time::now();
        if(now.sec - begin.sec >= time_out)
        {
    
    
            ROS_INFO_STREAM("Time out !");
            pubCommand = false;
            msg = "Time out !";
            break;
        }
        if(angle_fabs <= 3.1415926 && add_angle > 0)
        {
    
    
            if(angle_fabs < add_angle-0.5){
    
    
                angular_velocity_pub.publish(ang_msg);}
            else if(angle_fabs < add_angle)
            {
    
    
                definite_ang.angular.z = ang_msg.angular.z*(add_angle-angle_fabs)/0.5;
                if(definite_ang.angular.z > -0.06 && definite_ang.angular.z < 0)
                {
    
    definite_ang.angular.z = -0.05;}
                if(definite_ang.angular.z < 0.06 && definite_ang.angular.z > 0)
                {
    
    definite_ang.angular.z = 0.05;}
                angular_velocity_pub.publish(definite_ang);
            }
            else
            {
    
    
                finalangle = nowangle;
                pubCommand = true;
                msg = "Change the state successfully !";
                ROS_INFO_STREAM("The initial position yaw :"<<firstangle
                                                             <<" The final position yaw :"<<finalangle);
                break;
            }
        }
        else if(angle_fabs > 3.1415926 && add_angle > 0)
        {
    
    
            if(6.2831852-angle_fabs < add_angle-0.5){
    
    
                angular_velocity_pub.publish(ang_msg);}
            else if(6.2831852-angle_fabs < add_angle)
            {
    
    
                definite_ang.angular.z = ang_msg.angular.z*(add_angle-(6.2831852-angle_fabs))/0.5;
                if(definite_ang.angular.z > -0.06 && definite_ang.angular.z < 0)
                {
    
    definite_ang.angular.z = -0.05;}
                if(definite_ang.angular.z < 0.06 && definite_ang.angular.z > 0)
                {
    
    definite_ang.angular.z = 0.05;}
                angular_velocity_pub.publish(definite_ang);
            }
            else
            {
    
    
                finalangle = nowangle;
                pubCommand = true;
                msg = "Change the state successfully !";
                ROS_INFO_STREAM("The initial position yaw :"<<firstangle
                                                             <<" The final position yaw :"<<finalangle);
                break;
            }            
        }
        else
        {
    
    
            pubCommand = false;
            msg = "Input error !";
            break;
        }
        count++;
        loop_rate.sleep();
    }
    res.success = pubCommand;
    res.message = msg;
    return true;
}


void imuCallback(const sensor_msgs::Imu angle)
{
    
    
    tf::Quaternion quat;
    quat = tf::Quaternion(angle.orientation.x,angle.orientation.y,angle.orientation.z,angle.orientation.w);
    double roll,pitch,yaw;
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
    nowangle = yaw;
}

int main(int argc, char **argv)
{
    
    
    ros::init(argc,argv,"rotate_server");
    ros::NodeHandle n;
    ros::ServiceServer rotate_service = n.advertiseService("/rotate", rotateCallback);
    imu_sub = n.subscribe("/imu", 10, imuCallback);
    angular_velocity_pub = n.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity",10);
    ROS_INFO("Ready to receive command"); 
    ros::spin();
    return 0;
}

小结

周末愉快！