ROS动作编程（小海龟运动）

一、创建动作编程文件

1、创建服务文件

新建终端1，进入src文件

cd ~/ros/src/comm/src

ros为工作空间，comm为工程包

新建服务文件

touch turtleMove.cpp

打开文件

gedit turtleMove.cpp

编写代码

注意代码中工作空间和工程包名称问题，以自己的名称修改

/*  
　　　此程序通过通过动作编程实现由client发布一个目标位置
　　　然后控制Turtle运动到目标位置的过程
 */
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h> 
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
 
typedef actionlib::SimpleActionServer<comm::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 comm::turtleMoveGoalConstPtr& goal, Server* as)
{
    comm::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");
    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;
}

2、创建位置发布程序

创建文件

touch turtleMoveClient.cpp

打开文件

gedit turtleMoveClient.cpp

编写代码

#include <actionlib/client/simple_action_client.h>
#include "comm/turtleMoveAction.h"
#include <turtlesim/Pose.h> 
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h> 
typedef actionlib::SimpleActionClient<comm::turtleMoveAction> Client;
struct Myturtle
{
    float x;
    float y;
    float theta;
}turtle_present_pose;
 
// 当action完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,
        const comm::turtleMoveResultConstPtr& result)
{
    ROS_INFO("Yay! The turtleMove is finished!");
    ros::shutdown();
}
 
// 当action激活后会调用该回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}
 
// 收到feedback后调用该回调函数
void feedbackCb(const comm::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, "turtleMoveClient");
 
    // 定义一个客户端
    Client client("turtleMove", true);
 
    // 等待服务器端
    ROS_INFO("Waiting for action server to start.");
    client.waitForServer();
    ROS_INFO("Action server started, sending goal.");
 
    // 创建一个action的goal
    comm::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;
}

## 3、创建action文件夹
进入工程包

cd ~/ros/src/comm

创建文件夹

mkdir action

4、创建turtleMove.action文件

进入action文件夹

cd action

创建文件

touch turtleMove.action

打开文件

gedit 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文件

进入工程包

cd ~/ros/src/comm

打开CMakeLists.txt文件

sudo gedit CMakeLists.txt

添加代码至文件尾

add_executable(turtleMoveClient src/turtleMoveClient.cpp)
target_link_libraries(turtleMoveClient ${catkin_LIBRARIES})
add_dependencies(turtleMoveClient ${PROJECT_NAME}_gencpp)
add_executable(turtleMove src/turtleMove.cpp)
target_link_libraries(turtleMove ${catkin_LIBRARIES})
add_dependencies(turtleMove ${PROJECT_NAME}_gencpp)

同时修改find_package函数

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation
  actionlib_msgs
  actionlib
)

修改add_action_files函数

修改generate_messages函数

generate_messages(
   DEPENDENCIES
   std_msgs
   actionlib_msgs
 )

修改catkin_package函数

#  INCLUDE_DIRS include
#  LIBRARIES comm
#  CATKIN_DEPENDS roscpp rospy std_msgs
#  DEPENDS system_lib
 CATKIN_DEPENDS roscpp rospy std_msgs
  message_runtime

CMakeLists.txt修改完成！

三、修改package.xml文件

打开文件

gedit package.xml

替换build_depend一栏

<buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>actionlib_msgs</build_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>

替换exec_depend一栏

<exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>message_runtime</exec_depend>
  <exec_depend>actionlib</exec_depend>
  <exec_depend>actionlib_msgs</exec_depend> 

修改package.xml文件完成！

四、运行程序

编译程序

cd ~/ros
catkin_make

注册程序

 source ./devel/setup.bash

新建终端2，启动ROS

roscore

新建终端3，运行小海龟

rosrun turtlesim turtlesim_node

新建终端4，运行位置程序
1）注册程序

cd ~/ros
source ./devel/setup.bash

2）运行turtleMoveClient.cpp

输入命令但不回车确认

rosrun comm turtleMoveClient

测试程序
1）终端1运行turtleMove

输入命令但不回车确认

rosrun comm turtleMove

2）终端1确认，终端4确认，测试进行

实验结果：终端4在实时更新小乌龟的位置，而窗口3则显示小乌龟的移动路径

3）终端1ctrl+c结束程序

五、参考与总结

参考
https://blog.csdn.net/qq_42451251/article/details/104747355
总结
体验动作编程，实现了小海龟的位置的移动