ROS动作编程：客户端控制机器人到达目标位置-优快云博客

本文链接：https://blog.youkuaiyun.com/qq_53203987/article/details/129630958

一.ROS动作编程：客户端发送一个运动坐标，模拟机器人运动到目标位置的过程

环境准备

创建工作空间

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace

在这里插入图片描述

编译工作空间

cd ~/catkin_ws/
catkin_make

在这里插入图片描述

设置环境变量

source devel/setup.bash
echo $ROS_PACKAGE_PATH

在这里插入图片描述

创建功能包

cd ~/catkin_ws/src
catkin_create_pkg ros_communication std_msgs rospy roscpp
cd~/catkin_ws

在这里插入图片描述

编译功能包

catkin_make
source ~/catkin_ws/devel/setup.bash

在这里插入图片描述

编写程序

创建turtle.cpp文件

cd ~/catkin_ws/src/ros_communication/src
touch turtleMove.cpp
gedit turtleMove.cpp

在这里插入图片描述

在创建的turtleMove.cpp文件写入如下代码

#include<ros/ros.h>
#include<actionlib/server/simple_action_server.h>
#include"ros_communication/turtleMoveAction.h"
#include<turtlesim/Pose.h>
#include<turtlesim/Spawn.h>
#include<geometry_msgs/Twist.h>
typedef actionlib::SimpleActionServer<ros_communication::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 ros_communication::turtleMoveGoalConstPtr &goal, Server* as)
{
ros_communication::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);