ROS学习：机械臂抓取自定义消息数据类型，话题msg和服务srv通信（C++实现）

本文链接：https://blog.csdn.net/ssz__/article/details/129783062

创建文件

mkdir -p ros_basic/src
cd ros_basic/
catkin init
catkin build
catkin create pkg ros_define_data --catkin-deps std_msgs roscpp rospy

话题通信

grasp.msg文件代码如下

uint16 x
uint16 y
float64 z
float64 angle
float64 width

发布话题

#include "ros/ros.h"
#include <ros_define_data/grasp.h>

int main(int argc, char *argv[])
{
    ros::init(argc,argv,"grasp_publisher");
    ros::NodeHandle n;
    ros::Publisher grasp_info_pub = n.advertise<ros_define_data::grasp>("/grasp_info",10);
    ros::Rate loop_rate(1);

    int count = 0;
    while(ros::ok())
    {
        // 初始化ros_define_data::grasp类型的消息
        ros_define_data::grasp grasp_msg;
        grasp_msg.x = 100;
        grasp_msg.y = 181;
        grasp_msg.z = 0.324;
        grasp_msg.angle = 1.252;
        grasp_msg.width = 3.05;

        // 发布消息
        grasp_info_pub.publish(grasp_msg);

        ROS_INFO("Subcribe grasp Info: pos:(%d,%d),depth:%f,angle:%f,width:%f",
            grasp_msg.x,grasp_msg.y,grasp_msg.z,grasp_msg.angle,grasp_msg.width);
        
        // 按照循环频率延时
        loop_rate.sleep();
    }
    return 0;
}

订阅话题

#include "ros/ros.h"
#include <ros_define_data/grasp.h>

// 接受到订阅的消息后，会进入消息回调函数
void GraspInfoCallback(const ros_define_data::grasp::ConstPtr& msg){
    //将接受到的消息打印出来
    ROS_INFO("Subcribe grasp Info: pos:(%d,%d),depth:%f,angle:%f,width:%f",
            msg->x,msg->y,msg->z,msg->angle,msg->width); 
}

int main(int argc, char *argv[])
{
    // 初始化ros节点
    ros::init(argc,argv,"grasp_subscriber");
    // 创建节点句柄
    ros::NodeHandle n;
    ros::Subscriber grasp_info_sub = n.subscribe("/grasp_info",10,GraspInfoCallback);
    
    // 循环等待回调函数
    ros::spin();
    
    return 0;
}

运行结果

服务通信

grasp_.srv文件代码如下

# request
uint16 x
uint16 y         # 键盘输入坐标点
---
# response
uint16 distance  # 几何距离

客户端

#include "ros/ros.h"
#include <cstdlib>
#include <ros_define_data/grasp_.h>

int main(int argc, char *argv[])
{
    // 初始化ros节点
    ros::init(argc,argv,"grasp_client");
    // 从命令行获取两个数
    if(argc!=3)
    {
        ROS_INFO("usage:grasp_client X Y");
        return 1;
    }
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建一个client
    // 消息类型为ros_define_data::grasp_
    ros::ServiceClient client=n.serviceClient<ros_define_data::grasp_>("computing_distance");

    // 创建ros_define_data::grasp_类型的server消息
    ros_define_data::grasp_ srv;
    srv.request.x = atoll(argv[1]);
    srv.request.y = atoll(argv[2]);

    // 发布service消息，等待计算结果反馈回来，call表示发布服务请求了
    if(client.call(srv)){
        ROS_INFO("Distance:%ld",(long int)srv.response.distance);
    }else{
        ROS_ERROR("Failed to call service computing_distance");
        return 1;
    }
    return 0;
}

服务端

#include "ros/ros.h"
// 当前include文件夹中为本地地址devel下的include
#include <ros_define_data/grasp_.h>

// server设置回调函数，输入req,输出res
bool addCallback(ros_define_data::grasp_::Request &req,
                 ros_define_data::grasp_::Response &res){
    // 将输入参数中的请求数据相加，结果放到应答恢复中
    res.distance = req.x * req.x + req.y * req.y;
    ROS_INFO("request: x = %ld,y = %ld",(long int)req.x,(long int)req.y);
    ROS_INFO("response: distance = %ld",(long int)res.distance);

    return true;
}

int main(int argc, char *argv[])
{
    // ros节点初始化
    ros::init(argc,argv,"computing_distance_server");
    // 创建节点句柄
    ros::NodeHandle n;
    // 创建名为computing_distance的server，注册回调函数
    ros::ServiceServer service = n.advertiseService("computing_distance",addCallback);

    // 循环等待回调函数
    ROS_INFO("ready to computing distance");
    ros::spin();
    return 0;
}

运行结果

先创建grasp.msg和grasp_.srv文件进行编译生成.h头文件

此时CMakeLists.txt文件配置如下

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  # 自定义消息时添加
  geometry_msgs
  message_generation
)

add_message_files(
  FILES
  grasp.msg
  # Message2.msg
)

add_service_files(
  FILES
  grasp_.srv
  # Service2.srv
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES ros_define_data
 CATKIN_DEPENDS roscpp rospy std_msgs geometry_msgs message_runtime
#  DEPENDS system_lib
)

自定义msg和srv编译生成的.h头文件路径如下图

把四个.h头文件复制粘贴到下图位置

.h文件移动后CMakeLists.txt文件配置

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  # 自定义消息时添加
  geometry_msgs
  message_generation
)

include_directories(
  include
  ${catkin_INCLUDE_DIRS}
)

add_executable(server src/server.cpp)
add_executable(client src/client.cpp)
add_executable(publisher src/publisher.cpp)
add_executable(subscriber src/subscriber.cpp)

target_link_libraries(server
  ${catkin_LIBRARIES}
)
target_link_libraries(client
  ${catkin_LIBRARIES}
)
target_link_libraries(publisher
  ${catkin_LIBRARIES}
)
target_link_libraries(subscriber
  ${catkin_LIBRARIES}
)

package.xml文件配置

  <!-- self define some message data -->
  <build_export_depend>message_generation</build_export_depend>
  <exec_depend>message_runtime</exec_depend>
  <build_depend>geometry_msgs</build_depend>
  <exec_depend>geometry_msgs</exec_depend>