【ROS学习笔记】bodyreader包

一、image_trans.cpp

#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "image_transport/image_transport.h"
image_transport::Publisher image_pub;

定义了一个名为image_pub的image_transport::Publisher对象，用于发布图像消息。

---

void ImageCallback(const sensor_msgs::ImageConstPtr &msg)
{
	//printf("11111\n");
	image_pub.publish(msg);
}

定义了一个名为ImageCallback的回调函数

它接收一个sensor_msgs::Image类型的图像消息作为参数，并将该消息发布到image_pub。

---

int main(int argc, char **argv)
{
  //定义两个std::string类型的变量input_topic和output_topic
  //用于存储输入和输出图像话题的名称。
  std::string input_topic;
  std::string output_topic;

  //用ros::init函数初始化ROS节点，并指定节点的名称为"CompressedImage"。
  ros::init(argc, argv, "CompressedImage");

  //创建一个ros::NodeHandle对象nh，用于与ROS系统进行通信。~符号表示使用私有命名空间
  ros::NodeHandle nh("~");

  //使用nh.param函数从ROS参数服务器中获取输入和输出图像话题的名称
  //并将其存储到input_topic和output_topic变量中
  //如果参数服务器中没有指定这些参数的值，则使用默认值。
  nh.param<std::string>("input_image_topic", input_topic, "/body/image");
  nh.param<std::string>("output_image_topic", output_topic, "/repub/body/image");

  //创建一个image_transport::ImageTransport对象it，用于创建图像话题的订阅者和发布者
  image_transport::ImageTransport it(nh);

  //创建一个image_transport::Subscriber对象image_sub
  //并使用it.subscribe函数订阅输入图像话题，并指定回调函数为ImageCallback。
  image_transport::Subscriber image_sub;
  //image_transport::Publisher image_pub;
  image_sub = it.subscribe(input_topic, 1, ImageCallback);

  //使用it.advertise函数创建了一个发布器image_pub，用于发布输出图像话题。
  image_pub = it.advertise(output_topic, 1);

  //调用ros::spin函数来开始循环处理ROS消息，并使节点保持运行状态
  ros::spin();
  return 0;
}

二、interaction.cpp

#include <ros/ros.h>
...
ros::Publisher cmd_vel_Pub;
ros::Publisher fall_Pub;
int mode = 1;

string if_akm;

定义了两个ros::Publisher类型的发布者对象cmd_vel_Pub和fall_Pub，用于发布geometry_msgs::Twist类型和std_msgs::Int8类型的消息。

定义了一个int类型的变量mode，用于存储模式信息。

定义了一个string类型的变量if_akm，用于存储是否启用if_akm的信息。

---

void bodyposture_Callback(bodyreader::bodyposture msg)
{
    if (mode == 1)
    {
        std_msgs::Int8 fall_msg;
        geometry_msgs::Twist cmd_vel_msg;
        if (msg.left_foot_up == 1 && msg.right_foot_up == 0)
        {
            cmd_vel_msg.linear.x = -0.15;
        }
        else if (msg.left_foot_up == 0 && msg.right_foot_up == 1)
        {
            cmd_vel_msg.linear.x = 0.15;
        }
        else if (msg.left_hand_raised == 0 && msg.right_hand_raised == 1)
        {
            cmd_vel_msg.angular.z = 0.2;
            if (ros::param::get("if_akm_yes_or_no", if_akm))
            {
                cmd_vel_msg.linear.x = 0.15;
            }
        }
        else if (msg.left_hand_raised == 1 && msg.right_hand_raised == 0)
        {
            cmd_vel_msg.angular.z = -0.2;
            if (ros::param::get("if_akm_yes_or_no", if_akm))
            {
                cmd_vel_msg.linear.x = 0.15;
            }
        }
        else if (msg.left_arm_out == 1 && msg.right_arm_out == 0)
        {
            if (ros::param::get("if_akm_yes_or_no", if_akm))
            {
                cmd_vel_msg.linear.y = 0;
            }
            else
            {
                cmd_vel_msg.linear.y = -0.1;
            }
        }
        else if (msg.left_arm_out == 0 && msg.right_arm_out == 1)
        {
            if (ros::param::get("if_akm_yes_or_no", if_akm))
            {
                cmd_vel_msg.linear.y = 0;
            }
            else
            {
                cmd_vel_msg.linear.y = 0.1;
            }
        }
        else if (msg.fall == 1)
        {
            fall_msg.data = 1;
        }
        fall_Pub.publish(fall_msg);
        cmd_vel_Pub.publish(cmd_vel_msg);
    }
}

定义了一个名为bodyposture_Callback的回调函数，它接收一个bodyreader::bodyposture类型的消息作为参数。

在函数内部，根据接收到的消息的不同情况，设置cmd_vel_msg的不同属性值。

如果mode为1，将fall_msg发布到fall_Pub，将cmd_vel_msg发布到cmd_vel_Pub。

---

void mode_Callback(std_msgs::Int8 msg)
{
    mode = msg.data;
}

定义了一个名为mode_Callback的回调函数，它接收一个std_msgs::Int8类型的消息作为参数。

将接收到的消息的data值赋给mode。

---

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "interaction");    //初始化ROS节点
    ros::NodeHandle nh; 

    cmd_vel_Pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
    fall_Pub = nh.advertise<std_msgs::Int8>("/buzzer_flag", 1);
    ros::Subscriber bodyposture_sub = nh.subscribe("/body_posture", 1, bodyposture_Callback);
    ros::Subscriber mode_sub = nh.subscribe("/mode", 1, mode_Callback);

在main函数中，首先初始化ROS节点，并指定节点的名称为"interaction"。

然后，创建一个ros::NodeHandle对象nh，用于与ROS系统进行通信。

使用nh.advertise函数创建了两个发布者对象cmd_vel_Pub和fall_Pub，分别发布geometry_msgs::Twist类型和std_msgs::Int8类型的消息。

使用nh.subscribe函数创建了两个订阅者对象bodyposture_sub和mode_sub，分别订阅"/body_posture"和"/mode"话题，并指定回调函数为bodyposture_Callback和mode_Callback。

---

三、main.cpp

#include <astra/capi/astra.h>
...
ros::Publisher bodylist_Pub;
ros::Publisher image_Pub;
ros::Publisher mask_Pub;

加入头文件，其中，<astra/capi/astra.h> 是用于使用 Orbbec Astra 深度相机的 API，<ros/ros.h> 是用于使用 ROS 的 API。

定义三个 ROS 发布者对象，用于发布不同类型的消息

---

void print_color(astra_colorframe_t colorFrame)
{
.....
}

将相机的颜色帧数据转换为 ROS 消息，并将其发布到 ROS 系统中。具体来说，该函数首先获取颜色帧数据的元数据和 RGB 数据指针，并从中提取出帧索引、宽度、高度和中心像素的 RGB 值。然后，该函数使用这些数据填充 ROS 消息对象，并将其发布到 ROS 系统中。

---

void output_body_mask(astra_bodyframe_t bodyFrame)
{
    ...
}

void output_bodyframe_info(astra_bodyframe_t bodyFrame)
{
...
}

void output_joint(const int32_t bodyId, const astra_joint_t* joint)
{
...
}

void output_bodies(astra_bodyframe_t bodyFrame)
{
...
}

void output_bodyframe(astra_bodyframe_t bodyFrame)
{
...
}

int mian()
{
...
}

主函数 main作为程序的入口点

---

四、CMakeLists.txt

1、使用 find_package 命令来查找 ROS 组件

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  geometry_msgs
  message_generation
  image_transport
)

2、链接一些库目录和文件。这些库包括 astra，astra_core 和 astra_core_api

link_directories(lib lib/Plugins lib/Plugins/obt/3rdparty/lib lib/Plugins/openni)

3、定义一些消息类型和生成消息所依赖的其他消息

add_message_files(
   FILES
   bodyposture.msg
   vector2f.msg
   vector3f.msg
   joint.msg
   body.msg
   bodyList.msg
   maskdata.msg
   lockedmask_w_h.msg
   locked_char_rgb.msg
 )

generate_messages(
  DEPENDENCIES
  std_msgs
 )

4、定义 catkin_package，该命令定义了该软件包的元数据，例如它所依赖的其他软件包

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

5、包含头文件

6、定义可执行文件和它们所依赖的其他文件

五个可执行文件：main、bodydata_process、follower、interaction 和 feedback。其中，每个可执行文件都依赖于不同的库和其他可执行文件

add_executable(main src/main.cpp)
add_dependencies(main ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(main ${catkin_LIBRARIES} astra astra_core astra_core_api)

add_executable(bodydata_process src/bodydata_process.cpp)
add_dependencies(bodydata_process ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(bodydata_process ${catkin_LIBRARIES})
dd_dependencies(follower ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(follower ${catkin_LIBRARIES})

add_executable(interaction src/interaction.cpp)
add_dependencies(interaction ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(interaction ${catkin_LIBRARIES})


add_executable(feedback src/feedback.cpp)
add_dependencies(feedback ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(feedback ${catkin_LIBRARIES})


add_executable(image_trans src/image_trans.cpp)
add_dependencies(image_trans ${${PROJECT_NAME}_EXPORTED_TARGETS})
target_link_libraries(image_trans ${catkin_LIBRARIES})