ROS发布和订阅自定义消息

最近打算搞一下V2X涉及到自定义ROS消息，因此复习一下，总的来说就靠着下面这个图来改

文件结构

测试代码：

planner_determiner.h

#ifndef PLANNER_DETERMINER_H
#define PLANNER_DETERMINER_H

#include 
#include 
#include 
#include 
#include 

class planner_determiner
{
private:
    ros::NodeHandle nh_;

    pub_sub_test::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 pub_sub_test::adm_lat patrol_amd_lat);
    void get_behavior_sys_cmd(const pub_sub_test::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<pub_sub_test::adm_lat>("/AdmLatMsg", 1);
    patrol_sub_ = nh_.subscribe<pub_sub_test::adm_lat>("adm_info", 1, &planner_determiner::get_patrol_amd_lat, this);
    behavior_sub_ = nh_.subscribe<pub_sub_test::statemachine>("stateMachine_info", 1, &planner_determiner::get_behavior_sys_cmd, this);
}

planner_determiner::~planner_determiner()
{
}

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

void planner_determiner::get_patrol_amd_lat(const pub_sub_test::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 

planner_determiner.cpp

#include 
#include "../include/pub_sub_test/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;
}

talker1.cpp

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

    首先虚拟两个talker，就是在虚拟发送上面说的两个msg
*/

#include  //调用ros的c++接口
#include 

int main(int argc, char **argv)
{

    ros::init(argc, argv, "talker"); // 解析传入的ROS参数，定义节点名称
    ros::NodeHandle nh;              // 初始化节点，创建句柄对象实例化
    //  创建publisher，消息类型：test_pkg::gps ，定义topic:"gps_info"，消息队列的最大长度为1,一般取一个比较小的值
    pub_sub_test::adm_lat msg;
    ros::Publisher pub = nh.advertise<pub_sub_test::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("talker inline: %f", msg.V_des);
        pub.publish(msg);  // 发布msg
        loop_rate.sleep(); // 设置休眠
    }
    return 0;
}

talker2.cpp

#include 
#include 

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

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

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(pub_sub_test)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  message_generation  #new
)

add_message_files(
  FILES
  adm_lat.msg #new
  statemachine.msg #new
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
  CATKIN_DEPENDS roscpp rospy std_msgs message_runtime #new
)

include_directories(
  ${catkin_INCLUDE_DIRS}
)

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

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

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>pub_sub_test</name>
  <version>0.0.0</version>
  <description>The pub_sub_test package</description>

  <maintainer email="[email protected]">nvidia</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>message_generation</build_depend> <!-- new -->
  <build_depend>std_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> <!-- new -->


  <export>

  </export>
</package>

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_ASDmode
float32 ADM_PosLon
float32 ADM_PosLat
uint8 ADM_Fuel_Signal

参考链接1
参考链接2