ROS 快速入门教程05

15. IMU航向锁定的节点

编写锁定节点

打开vscode编写imu_node.cpp

#include<ros/ros.h>
#include<sensor_msgs/Imu.h>
#include<tf/tf.h>
#include<geometry_msgs/Twist.h>

ros::Publisher vel_pub;

void IMUCallback(sensor_msgs::Imu msg)
{
  if(msg.orientation_covariance[0]<0)
  {
    ROS_INFO("IMU数据不可用");
    return;
  }
  tf::Quaternion quaternion(
    msg.orientation.x, 
    msg.orientation.y,
    msg.orientation.z,
    msg.orientation.w
    );
    double roll, pitch, yaw;
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);
    roll = roll * 180 / M_PI;
    pitch = pitch * 180 / M_PI;
    yaw = yaw * 180 / M_PI;
    ROS_INFO("滚转角: %.0f, 俯仰角: %.0f, 朝向: %.0f", roll, pitch, yaw);

    double target_yaw = 90; // 目标偏航角
    double diff_angle = target_yaw - yaw; // 计算偏航角差
    geometry_msgs::Twist vel_cmd;
    vel_cmd.angular.z = diff_angle*0.01;
    vel_cmd.linear.x=0.1;// 设置线速度
    vel_pub.publish(vel_cmd);  // 发布速度指令
  }

int main(int argc, char** argv)
{
    setlocale(LC_ALL, "");//设置中文显示
    ros::init(argc, argv, "imu_node");//初始化节点
    ros::NodeHandle nh;//创建节点句柄
    ros::Subscriber imu_sub = nh.subscribe("/imu/data", 10, IMUCallback);//订阅IMU数据 {
    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);//发布速度指令
    ros::spin();//循环等待回调函数
    return 0;
}

打开两个终端分别启动节点和仿真环境

roslaunch wpr_simulation wpb_simple.launch 
rosrun imu_pkg imu_node

可以观察到小车朝向不断调整至90度

16.ROS 的标准消息包 std_msgs

17.ROS 中的几何包 geometry_msgs 和 传感器包 sensor_msgs

17.1 几何消息包

17.2 传感器消息包

18. ROS中生成自定义消息类型

cd catkin_ws/src/
catkin_create_pkg qq_msgs roscpp rospy std_msgs message_generation message_runtime

vscode打开qq_msgs，新建文件夹msg，新建文件carry.msg

string grade
int64 star
string data

打开CmakeList文件：

find_package(catkin REQUIRED COMPONENTS
  message_generation//确保里面有这两个
  message_runtime//确保里面有这两个
  roscpp
  rospy
  std_msgs
)
add_message_files(
  FILES
  Carry.msg
)//添加自定义消息包

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
  CATKIN_DEPENDS message_generation message_runtime roscpp rospy std_msgs
)

打开package.xml补全

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

在工作空间catkin_make编译文件后

rosmsg show qq_msgs/Carry 

18.1 小结

19.在ROS中使用自定义消息类型

• 发布者节点的编译：将过去写好的chao_node.cpp进行重新编写。

#include<ros/ros.h>
#include<std_msgs/String.h>
#include<qq_msgs/Carry.h>//自定义消息

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "chao_node");
    printf("你已经成功编译了一个ROS节点\n");

    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<qq_msgs::Carry>("chao_node", 10);//发布话题
    ros::Rate loop_rate(10);
    while(ros::ok())
    {
       printf("你已经成功编译了一个ROS节点！\n");
       qq_msgs::Carry msg;
       msg.grade = "king";//自定义消息
       msg.star =50;//自定义消息
       msg.data = "Hello, ROS!";
       pub.publish(msg);
       loop_rate.sleep();
    }
    return 0;
}

• Cmake文件：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  qq_msg//添加
)

//添加
dd_dependencies(chao_node qq_msgs_generate_messages_cpp) 

• xml文件添加

 <build_depend>qq_msgs</build_depend>
 <exec_depend>qq_msgs</exec_depend>

打开终端catkin_make进行编译。

• 订阅者节点编译，将过去写好的ma_node.cpp进行重新编写。

#include<ros/ros.h>
#include<std_msgs/String.h>
#include<qq_msgs/Carry.h>//自定义消息

void chao_callback( qq_msgs::Carry msg)
{
  ROS_WARN(msg.grade.c_str());
  ROS_WARN("star: %d星", msg.star);
  ROS_INFO(msg.data.c_str());
}

void yao_callback(std_msgs::String msg)
{
  ROS_WARN(msg.data.c_str());
}
int main(int argc, char **argv)
{
  setlocale(LC_ALL,"");//设置中文显示
  ros::init(argc, argv, "ma_node");
  ros::NodeHandle nh;
  ros::Subscriber sub = nh.subscribe("chao_node", 10,chao_callback);
  ros::Subscriber sub1 = nh.subscribe("yao_node", 10,yao_callback);
  while(ros::ok())
  {
    ros::spinOnce();//功能：处理回调函数
  }
     return 0;
}

• Cmake文件：

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  qq_msg//添加
)

//添加
dd_dependencies(ma_node qq_msgs_generate_messages_cpp) 

• xml文件添加

 <build_depend>qq_msgs</build_depend>
 <exec_depend>qq_msgs</exec_depend>

打开终端catkin_make进行编译。
打开三个终端分别输入：

roscore
rosrun ssr_pkg chao_node
rosrun atr_pkg ma_node

19.1小结

20.ROS中的栅格地图格式

在 ROS（Robot Operating System）中，栅格地图是最常用的一种环境表示方法，它将机器人周围的空间划分成一系列规则的、等尺寸的网格单元（cells），并在每个单元中存储关于该区域的状态信息（如是否可通行、是否被障碍物占据等）。

• 栅格化
  将连续的二维平面离散化为 $M\times N$ 个正方形或长方形单元，每个单元称为“栅格（cell）”。
• 分辨率（resolution）
  每个栅格边长对应的实际距离，单位通常为米 (m)。例如分辨率为 0.05 m，表示每个栅格代表 $5$ cm × $5$ cm 的区域。
• 地图原点（origin）
  栅格地图的世界坐标系原点通常在矩阵的左下角，对应于 origin 字段中的 $(x,y,\theta )$；$\theta$ 表示地图坐标系相对于世界坐标系的旋转角度。
• 静态地图（Static Map）
  • 通常由人工测绘或激光雷达扫描后离线构建；
  • 通过 map_server 节点加载 yaml + pgm 文件后，发布全局静态栅格；
    在线建图（SLAM）
  • 如 gmapping、hector_slam、cartographer 等，边移动边构建 OccupancyGrid；
  • 适合未知环境的自主探索。
    -
    

• 编写map_pub_node.cpp节点

#include<ros/ros.h>
#include<nav_msgs/OccupancyGrid.h>
int main(int argc, char **argv){
    ros::init(argc, argv, "map_pub_node");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<nav_msgs::OccupancyGrid>("/map", 10);
    ros::Rate loop_rate(1); // 1 Hz
    while(ros::ok()){
      nav_msgs::OccupancyGrid msg;
      msg.header.frame_id = "map";
      msg.header.stamp = ros::Time::now();
      msg.info.origin.position.x = 0.0;
      msg.info.origin.position.y = 0.0;
      msg.info.resolution = 1.0;
      msg.info.width = 4;
      msg.info.height = 2;

      msg.data.resize(4*2);
      msg.data[0] = 100;
      msg.data[1] = 100;
      msg.data[3] = -1;
      pub.publish(msg);
      loop_rate.sleep();
    }
     return 0;
 }

编辑Cmake依赖

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

打开终端catkin_make进行编译。
打开三个终端分别输入：

roscore
rosrun map_pkg map_pub_node 
rviz