【ros】控制小海龟定点移动

本文使用ros1

---

前提配置

工作空间名称：catkin_ws

cd ~/catkin_ws/src
catkin_create_pkg learning_client roscpp rospy std_msgs geometry_msgs turtlesim

功能包名称:learning_client
learning_client包括下列文件夹：include launch src srv

一、客户端与服务端的通讯类型

本文将指导如何建立自定义的通讯类型

cd ~/catkin_ws/src/learning_client/srv
gedit SrvTutorial.srv

键入并保存：

float32 x
float32 y
---
float32 ready_wh

对cmakelist.txt 进行配置

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  geometry_msgs
  message_generation
)
# 在依赖后面添加了message_generation
add_service_files(
  FILES
  SrvTutorial.srv
)
generate_messages(
  DEPENDENCIES
  std_msgs
)
catkin_package(
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
)
include_directories(
  ${catkin_INCLUDE_DIRS}
)

对package.xml进行配置，添加下述即可

 <build_depend>message_generation</build_depend>
  <exec_depend>message_runtime</exec_depend>

二、服务端的创建

在learning_client的src下创立s_v.cpp文件。
文件代码如下:

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/PointStamped.h"
#include<iostream>
#include <geometry_msgs/Twist.h>
#include<learning_client/SrvTutorial.h>
// g_x,g_y 小海龟应该具有的线速度 n_x,n_y 目前所在地点
float g_x,g_y,n_x,n_y;
//用于记录是否监听到客户端传递的消息
bool fl_g=0;
//回调函数，订阅小海龟目前所在位置
void doPose(const turtlesim::Pose::ConstPtr &pose)
{
    std::cout<<"t_x = "<<pose->x<<std::endl;
    std::cout<<"t_y = "<<pose->y<<std::endl;
    n_x = pose->x;
    n_y = pose->y;
}
//回调函数，用于
bool doReq(learning_client::SrvTutorial::Request &req,learning_client::SrvTutorial::Response &res)
{
    ROS_INFO("ready");
    g_x = req.x - n_x;
    g_y = req.y - n_y;
    fl_g=1;
return 0;
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 初始化 ROS 节点
    ros::init(argc,argv,"v_server");
    // 创建 ROS 句柄
    ros::NodeHandle n;
    //订阅小海龟的位置
    ros::Subscriber sub = n.subscribe("/turtle1/pose",100,doPose);
    //发布小海龟的控制指令
    ros::Publisher t_vel_pub =n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
	//注册服务端和客户端
    ros::ServiceServer turtle_vel_p = n.advertiseService("/tell",doReq);
    geometry_msgs::Twist vel_msg;
    while(ros::ok())
	{
    ros::spinOnce();
    //fl_g标志是否传入目标地点的信息
    if(fl_g)
	    {
        fl_g=0;
        vel_msg.linear.x= 1*g_x;  //线速度
        vel_msg.linear.y= 1*g_y;
        //发布给小海龟控制信息
        t_vel_pub.publish(vel_msg);
   		}
	}
	//开始处理回调函数
    ros::spin();
    return 0;
}

在CmakeLists.txt的Build模块如下：

add_executable(s_v src/s_v.cpp)
target_link_libraries(s_v ${catkin_LIBRARIES})

三、客户端的创建

同样在srv文件夹下创建g_v.cpp

#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "geometry_msgs/PointStamped.h"
#include<iostream>
#include <geometry_msgs/Twist.h>
#include<learning_client/SrvTutorial.h>
int main(int argc,char *argv[])
{
    setlocale(LC_ALL,"");
    //初始化 ROS 节点
    ros::init(argc,argv,"g_client");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    //等带接受/tell的服务端出现，若没有此处也可，只是会有warning出现
    ros::service::waitForService("/tell");

    // 4.创建 客户端 对象
    ros::ServiceClient client = nh.serviceClient<learning_client::SrvTutorial>("/tell");


	while(ros::ok())
	{
		ros::spinOnce();
        learning_client::SrvTutorial pose;
        float x=0;
        float y=0;
        //键入要到达的目标地点
        ROS_INFO("x = ");
        std::cin>>x;
        ROS_INFO("y = ");
        std::cin>>y;
        ROS_INFO("I got it");
		pose.request.x=x;
		pose.request.y=y;
        client.call(pose);
	}
    ros::spin();
    return 0;
}

在CmakeLists.txt的Build模块如下：

add_executable(g_v src/g_v.cpp)
target_link_libraries(g_v ${catkin_LIBRARIES})

四、launch文件

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="tutle_GUI"/>
    <node pkg="learning_client" type="g_v" name="client" output="screen" launch-prefix="gnome-terminal -x" />
    <node pkg="learning_client" type="s_v" name="s_pub" output="screen" launch-prefix="gnome-terminal -x" />
</launch>

启动launch文件即可