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的goallearn_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

运行结果如下：

ROS学习记录：动作编程相关推荐

ros学习记录：Gazebo加载速度慢，长时间停在“Preparing your world”
前言 ros学习记录笔记,一个小白的自留地,欢迎大佬批评指正~ 一.问题描述 Gazebo加载速度慢,长时间停在"Preparing your world" 二.解决办法 1.下载 ...
Webots+ROS学习记录（4）——六轮全地形移动机器人
Webots+ROS学习记录(4)--六轮全地形移动机器人有了以上经验,可以创造出一个全地形的移动机器人如图1 第一步,创建robot节点,并给robot节点编写相应参数注意,这里的机身不能再使用 ...
ROS学习记录（二）阿克曼转向车运动学模型及在gazebo中搭建仿真环境
前言:该篇是ROS学习记录的第二篇,如果还没关注过之前文章的读者,如有需要可以通过下方传送门去看之前的文章: ROS学习记录(一) Plugin插件这两天关注了古月老师的公众号,看到了其中一篇课程推 ...
ROS学习记录9——urdf文件的创建与使用
零.前言在学习这一节之前,至少得明白如何创建一个工作空间,如何编译工作空间,如何运行工作空间里的文件(把工作空间的环境目录添加到环境里).如果有不会的,请看鄙人的ROS学习记录3. 本节默认工作空间 ...
【ROS学习记录】2021/7/2 Gazebo+rviz仿真（一）
[ROS学习记录]2021/7/2 Gazebo+rviz进行turtlebot2仿真问题记录 ##小插曲今天ubuntu挂载u盘一直读取失败所以做了一些令人窒息的操作,晚上开机就进入了initra ...
ROS学习记录 | 便于回顾
ROS学习记录 | 便于回顾 ROS环境搭建 ROS部分基础知识 ROS环境搭建 step1 创建工作空间目录结构 mkdir catkin_ws cd catkin_ws mkdir src cd ...
ros学习记录1 Hello World 使用c++
声明:整个学习是我跟着奥特学园进行的,不是原创,只是记录自己的学习过程步骤创建命名空间 mkdir -p catkin_ws/src //其中前者是工作空间的名称,后者为存放源码的位置 cd ./ ...
ROS学习记录（二）玩转海龟——海龟保姆级教程
前言--环境变量那些事儿编译工作空间后,一定要先设置环境变量,此工作空间下的代码才能有效运行在工作空间目录下打开终端,输入 source devel/setup.bash 此次source仅对当前 ...
ROS学习记录（一） Plugin插件
前言:最近在学习无人驾驶路径规划和ROS的一些基础理论,过程中遇到了一些问题,写这个系列记录一下个人的学习过程. 遇到这个问题的初衷是看了小巨同学一篇帖子使用自定义RRT*全局规划器建图导航文中提 ...
ROS学习记录（七）连接单片机
这次的记录来源于最近在把ROS和STM32联系在一起,更多是填坑吧. 一.使用串口联系ROS和下位机主要参考: ROS中串口操作中间遇到了一个问题,按照上一个文章进行操作,总会打开串口失败, 解决 ...