ROS学习记录：动作编程

动作编程:客户端发送一个运动目标,模拟机器人运动到目标位置的过程,包含服务端和客户端的代码实现，要求带有实时位置反馈

一、在工作空间下创建功能包

cd ~/catkin_ws/src
catkin_create_pkg learn_action std_msgs rospy roscpp actionlib actionlib.msgs

二、在创建的功能包learn_action的src目录下创建CPP文件

TurtleMove_client.cpp:

#include <actionlib/client/simple_action_client.h>
#include "learn_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

typedef actionlib::SimpleActionClient<learn_action::TurtleMoveAction> Client;

struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_present_pose;

// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const learn_action::TurtleMoveResultConstPtr& result)
{
    ROS_INFO("Yay! The TurtleMove is finished!");
    ros::shutdown();
}

// 当action激活后会调用该回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}

// 收到feedback后调用该回调函数
void feedbackCb(const learn_action::TurtleMoveFeedbackConstPtr& feedback)
{
    ROS_INFO(" present_pose : %f  %f  %f", feedback->present_turtle_x,
                   feedback->present_turtle_y,feedback->present_turtle_theta);
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "TurtleMove_client");

    // 定义一个客户端
    Client client("TurtleMove", true);

    // 等待服务器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    // 创建一个action的goal
    learn_action::TurtleMoveGoal goal;
    goal.turtle_target_x = 1;
    goal.turtle_target_y = 1;
    goal.turtle_target_theta = 0;

    // 发送action的goal给服务器端，并且设置回调函数
    client.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

    ros::spin();

    return 0;
}

TurtleMove_server.cpp:

/*  
　　　此程序通过通过动作编程实现由client发布一个目标位置
　　　然后控制Turtle运动到目标位置的过程
 */
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "learn_action/TurtleMoveAction.h"
#include <turtlesim/Pose.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>

typedef actionlib::SimpleActionServer<learn_action::TurtleMoveAction> Server;

struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_original_pose,turtle_target_pose;

ros::Publisher turtle_vel;

void posecallback(const turtlesim::PoseConstPtr& msg)
{
  ROS_INFO("Turtle1_position:(%f,%f,%f)",msg->x,msg->y,msg->theta);
  turtle_original_pose.x=msg->x;
  turtle_original_pose.y=msg->y;
  turtle_original_pose.theta=msg->theta;
 }

// 收到action的goal后调用该回调函数
void execute(const learn_action::TurtleMoveGoalConstPtr& goal, Server* as)
{
    learn_action::TurtleMoveFeedback feedback;
     ROS_INFO("TurtleMove is working.");
    turtle_target_pose.x=goal->turtle_target_x;
    turtle_target_pose.y=goal->turtle_target_y;
    turtle_target_pose.theta=goal->turtle_target_theta;

    geometry_msgs::Twist vel_msgs;
    float break_flag;

    while(1)
    {
        ros::Rate r(10);

        vel_msgs.angular.z = 4.0 * (atan2(turtle_target_pose.y-turtle_original_pose.y,
                                   turtle_target_pose.x-turtle_original_pose.x)-turtle_original_pose.theta);
        vel_msgs.linear.x = 0.5 * sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
                                      pow(turtle_target_pose.y-turtle_original_pose.y, 2));
        break_flag=sqrt(pow(turtle_target_pose.x-turtle_original_pose.x, 2) +
                                        pow(turtle_target_pose.y-turtle_original_pose.y, 2));
        turtle_vel.publish(vel_msgs);

        feedback.present_turtle_x=turtle_original_pose.x;
        feedback.present_turtle_y=turtle_original_pose.y;
        feedback.present_turtle_theta=turtle_original_pose.theta;
        as->publishFeedback(feedback);
        ROS_INFO("break_flag=%f",break_flag);
        if(break_flag<0.1) break;
        r.sleep();
    }
        // 当action完成后，向客户端返回结果
        ROS_INFO("TurtleMove is finished.");
        as->setSucceeded();
}
int main(int argc, char** argv)
{
    ros::init(argc, argv, "TurtleMove_server");
    ros::NodeHandle n,turtle_node;
    ros::Subscriber sub = turtle_node.subscribe("turtle1/pose",10,&posecallback); //订阅小乌龟的位置信息
    turtle_vel = turtle_node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);//发布控制小乌龟运动的速度
    // 定义一个服务器
        Server server(n, "TurtleMove", boost::bind(&execute, _1, &server), false);
        // 服务器开始运行
        server.start();
        ROS_INFO("server has started.");
    ros::spin();

    return 0;
}

三、创建action文件

在功能包下创建action文件夹

文件夹内创建TurtleMove.action文件

TurtleMove.action内容：

# Define the goal
float64 turtle_target_x  # Specify Turtle's　target position
float64 turtle_target_y
float64 turtle_target_theta
---
# Define the result
float64 turtle_final_x
float64 turtle_final_y
float64 turtle_final_theta
---
# Define a feedback message
float64 present_turtle_x
float64 present_turtle_y
float64 present_turtle_theta

四、在CmakeLists.txt文件中添加依赖

找到对应的地方做如下修改：



然后在文件末尾添加依赖项：

add_executable(TurtleMove_client src/TurtleMove_client.cpp)
target_link_libraries(TurtleMove_client ${catkin_LIBRARIES})
add_dependencies(TurtleMove_client ${PROJECT_NAME}_gencpp)
 
add_executable(TurtleMove_server src/TurtleMove_server.cpp)
target_link_libraries(TurtleMove_server ${catkin_LIBRARIES})
add_dependencies(TurtleMove_server ${PROJECT_NAME}_gencpp)

修改package文件，添加如下代码：

<build_depend>message_generation</build_depend>

<exec_depend>message_runtime</exec_depend>

如下图：

五、编译程序

cd ~/catkin_ws
catkin_make

六、运行程序

新建终端，启动ROS核心程序

roscore

运行小海龟程序节点

rosrun turtlesim turtlesim.node

运行TurtleMove_server.cpp

source ./devel/setup.bash
rosrun learn_action TurtleMove_server

运行TurtleMove_client.cpp

source ./devel/setup.bash
rosrun learn_action TurtleMove_client

运行结果如下：