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 COMPONENTSmessage_generation//确保里面有这两个message_runtime//确保里面有这两个roscpprospystd_msgs
)
add_message_files(FILESCarry.msg
)//添加自定义消息包generate_messages(DEPENDENCIESstd_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 COMPONENTSroscpprospystd_msgsqq_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 COMPONENTSroscpprospystd_msgsqq_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 Hzwhile(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