ROS动作编程和bag文件的读取

ROS动作编程和bag文件的读取

1、实验内容

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

2.用 rviz 读入. bag 格式的 文件并播放！

2、实验过程

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

(1) 、在工程包中创建项动作编程需要的文件

创建小乌龟移动的“服务文件”turtleMove.cpp )

#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
}

创建小乌龟“发布目标位置文件”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
}

（2）、在功能包目录下创建 action 文件夹

cd learning_communication
mkdir action

（3）、在 action 文件夹下创建 turtleMove.action 文件

cd action
vim 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
- 7 -
float64 present_turtle_x
float64 present_turtle_y
float64 present_turtle_theta

（4）修改 CMakeLists.txt 文件内容

#返回上级目录
cd ..
vim 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 函数一项

add_action_files(
FILES
turtleMove.action
)

继续在该文件中找到 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

(5)修改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>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>

（6）运行测试文件

cd ~/catkin_ws
catkin_make
source ./devel/setup.bash

新建一个终端2，启动ros：

命令：roscore

新建一个终端3， 运行小海龟

仙剑一个终端4，运行我们的目标位置程序代码：

rosrun learning_communication turtleMoveClient

进行测试

在终端 1 运行 turtleMove

rosrun learning_communication turtleMove

开始测试，终端 1 回车、终端 4 回车，出现如下画面，则我们动作编程的实验 完美成功：

通过下图，我们可以看到，终端 4 在实时更新小乌龟的位置，而窗口 3 则显示 小乌龟的移动路径

bag文件的读取

安装rviz软件

sudo apt install ros-noetic-rviz

准备并下载一个bag文件，没有可以前往这个网站进行下载

http://download.ros.org/data/amcl/

将下载好的文件，并放入一个方便的文件夹下

打开终端，输入指令

roscore

新建终端，打开rviz软件

rosrun rviz rviz

输入命令后就会打开rviz软件

配置我们的rviz软件

选择左下角的 add 按钮点击

在出现的页面中选择 by display type,下拉选择 pointCloud2 后点击 ok

左侧的 Global Options,下面的 For frame，双击右侧进行修改

下面翻，找到 pointCloud2 栏目下的 Topic,修改为如下

打开我们的bag文件,输入命令进行播放

rosbag play .bag

在输入以上命令回车后，我们可以发现，程序正在运行

总结

本次的实验我也是遇到了很多的问题，但是也是一步步慢慢的进行了解决，最终也还是解决了这些问题。

主要是通过这次实验了解到了ros之间的动作编程，以及ros中rviz这个可视化工具的使用，已经对于rviz有了一点更加清晰的认知