ROS在类中发布和接受消息(自定义消息)

最新推荐文章于 2024-08-26 12:00:00 发布

gophae

最新推荐文章于 2024-08-26 12:00:00 发布

阅读量2.6k

点赞数 1

分类专栏： ROS

本文链接：https://blog.csdn.net/gophae/article/details/108103499

版权

ROS 专栏收录该内容

8 篇文章 3 订阅

订阅专栏

一开始建立pkg:

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/
catkin_make
cd ~/catkin_ws/src
catkin_create_pkg pub_sub_test std_msgs rospy roscpp
cd ..
catkin_make

我们现在有两个自定义消息：
adm_lat.msg

uint8 Enable_lat
float32 GPS1_Curvature_cmd
float32 V_des
int8 Gear_des
uint8 End
uint8 Objectfalse
float32 obs_dis
float32 steering_wheel_angle
float32 lateral_offset

statemachine.msg

int8 route
int8 ADM_command
int8 parking_control
int8 DiveroffReq
int8 ADM_SubtaskState
int8 planning_control
int8 DC_command
int8 dischargeReq
uint8 ADM_ADSmode
float32 ADM_PosLon
float32 ADM_PosLat
uint8 ADM_Fuel_Signal

在determiner里面通过判断statemachine中的parking_control来确定发送什么样的adm_lat信号出去，如果是1就原封不动发出去，如不是，就发四个固定值出去。

首先虚拟两个talker,就是在虚拟发送上面说的两个msg
talker1


#include <ros/ros.h>      / /调用ros的C++接口;
#include <VehicleMsg/adm_lat.h> //调用自定义msg 产生的头文件
int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker1"); //解析传入的ROS参数，定义节点名称
    ros::NodeHandle nh;               // 初始化节点，创建句柄对象（实例化）
    VehicleMsg::adm_lat msg;          //创建自定义gps消息，并初始化;
                                      //  创建publisher，消息类型：test_pkg::gps ，定义topic:"gps_info"，消息队列的最大长   度为1,一般取一个比较小的值
    ros::Publisher pub = nh.advertise<VehicleMsg::adm_lat>("adm_info", 1);
    ros::Rate loop_rate(1.0); //定义发布频率，1HZ
    while (ros::ok())
    {

        msg.Enable_lat = 0;
        msg.Gear_des = 0;
        msg.GPS1_Curvature_cmd = 0;
        msg.V_des = 123;
        ROS_INFO("talker1 online: %f", msg.V_des);
        pub.publish(msg);  //发布msg
        loop_rate.sleep(); //设置休眠;
    }
    return 0;
}

talker2

//消息发布节点：talker
//在 catkin_ws / src / test_pkg / src下，新建talker.cpp 文件:
#include <ros/ros.h>      / /调用ros的C++接口;
#include <StatemachineMsg/statemachine.h> //调用自定义msg 产生的头文件
int main(int argc, char **argv)
{
    ros::init(argc, argv, "talker2");  //解析传入的ROS参数，定义节点名称
    ros::NodeHandle nh;                // 初始化节点，创建句柄对象（实例化）
    StatemachineMsg::statemachine msg; //创建自定义gps消息，并初始化;
                                       //  创建publisher，消息类型：test_pkg::gps ，定义topic:"gps_info"，消息队列的最大长   度为1,一般取一个比较小的值
    ros::Publisher pub = nh.advertise<StatemachineMsg::statemachine>("stateMachine_info", 1);
    ros::Rate loop_rate(1.0); //定义发布频率，1HZ
    while (ros::ok())
    {
        //虚拟一个GPS位置信号

        msg.parking_control = 0;
        ROS_INFO("talker2 online: %f", msg.parking_control);
        pub.publish(msg);  //发布msg
        loop_rate.sleep(); //设置休眠;
    }
    return 0;
}

determiner.h

#ifndef PLANNER_DETERMINER_H
#define PLANNER_DETERMINER_H

#include "ros/ros.h"
#include <std_msgs/UInt8.h>
#include <VehicleMsg/adm_lat.h>
#include <boost/bind.hpp>
#include <StatemachineMsg/statemachine.h>

class planner_determiner
{
private:
    ros::NodeHandle nh_;

    VehicleMsg::adm_lat final_amd_lat_msg_;
    ros::Publisher final_amd_lat_pub_;
    ros::Subscriber patrol_sub_;
    ros::Subscriber dwa_sub_;
    ros::Subscriber behavior_sub_;

    int Enable_lat_ = 0;
    int Gear_des_ = 2;
    int GPS1_Curvature_cmd_ = 4;
    int V_des_ = 10;
    int planner_state_ = 0;

    void get_patrol_amd_lat(const VehicleMsg::adm_lat patrol_amd_lat);
    void get_behavior_sys_cmd(const StatemachineMsg::statemachine behavior_sys_cmd);

public:
    planner_determiner(ros::NodeHandle &node_handle);
    ~planner_determiner();
    void publishPlannerCmd();
};

planner_determiner::planner_determiner(ros::NodeHandle &node_handle) : nh_(node_handle)
{
    nh_ = node_handle;

    final_amd_lat_pub_ = nh_.advertise<VehicleMsg::adm_lat>("/AdmLatMsg", 1);
    patrol_sub_ = nh_.subscribe<VehicleMsg::adm_lat>("adm_info", 1, &planner_determiner::get_patrol_amd_lat, this);
    behavior_sub_ = nh_.subscribe<StatemachineMsg::statemachine>("stateMachine_info", 1, &planner_determiner::get_behavior_sys_cmd, this);
}

planner_determiner::~planner_determiner()
{
}

void planner_determiner::get_behavior_sys_cmd(const StatemachineMsg::statemachine behavior_sys_cmd)
{
    planner_state_ = behavior_sys_cmd.parking_control;
    return;
}

void planner_determiner::get_patrol_amd_lat(const VehicleMsg::adm_lat patrol_amd_lat)
{
    if (planner_state_ == 1)
    {
        final_amd_lat_msg_.Enable_lat = patrol_amd_lat.Enable_lat;
        final_amd_lat_msg_.Gear_des = patrol_amd_lat.Gear_des;
        final_amd_lat_msg_.GPS1_Curvature_cmd = patrol_amd_lat.GPS1_Curvature_cmd;
        final_amd_lat_msg_.V_des = patrol_amd_lat.V_des;
        ROS_INFO("fouth numbers : %f", final_amd_lat_msg_.V_des);
    }
    else
    {
        final_amd_lat_msg_.Enable_lat = Enable_lat_;
        final_amd_lat_msg_.Gear_des = Gear_des_;
        final_amd_lat_msg_.GPS1_Curvature_cmd = GPS1_Curvature_cmd_;
        final_amd_lat_msg_.V_des = V_des_;
        ROS_INFO(VehicleMsg::adm_lat);
        ROS_INFO("the 4 numbers : %f  %f  %d  %d", final_amd_lat_msg_.Enable_lat,
                 final_amd_lat_msg_.Gear_des,
                 final_amd_lat_msg_.GPS1_Curvature_cmd,
                 final_amd_lat_msg_.V_des);

        return;
    }
}

void planner_determiner::publishPlannerCmd()
{
    final_amd_lat_pub_.publish(final_amd_lat_msg_);
}

#endif

determiner.cpp

#include "ros/ros.h"
#include "planner_determiner.h"

int main(int argc, char **argv)
{
    ros::init(argc, argv, "planner_determiner");
    ros::NodeHandle nh;
    planner_determiner pd(nh);

    ros::Rate loop_rate(10);
    while (ros::ok())
    {
        ros::spinOnce();
        pd.publishPlannerCmd();
        loop_rate.sleep();
    }

    return 0;
}

rosrun两个talker, 并rosrun determiner可以看到信息收发。以及rqt_graph可以看到。
在这里插入图片描述
cmakelist里面：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
  VehicleMsg
  StatemachineMsg
message_generation
)

add_executable(talker1 src/talker1.cpp)
add_executable(talker2 src/talker2.cpp)
add_executable(planner_determiner src/planner_determiner.cpp)

add_dependencies(talker1  ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(talker2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(planner_determiner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

target_link_libraries(talker1 ${catkin_LIBRARIES})
target_link_libraries(talker2 ${catkin_LIBRARIES})
target_link_libraries(planner_determiner ${catkin_LIBRARIES})
)