车载软件开发基础

开启小车的激光雷达、深度相机等传感器，显示移动过程中，各类传感器数据：

命令行窗口显示小车的线速度和角速度

1、开启各类传感器：

开启小车的激光雷达：

roslaunch scout_bringup open_rslidar.launch

开启深度相机：

roslaunch realsense2_camera rs_camera.launch

开启imu单元：

roslaunch imu_launch imu_msg.launch

上面为本人学习时的启动launch包方法（提供的松灵小车），写在这里纯粹以便自己回顾。

2、查找需要订阅的线速度和角速度的节点或话题是什么

我在这里最开始一点也不了解，在看了autolabo教程2.5.2 实操02_话题订阅 · Autolabor-ROS机器人入门课程《ROS理论与实践》零基础教程和一些实践后发现灵活使用rostopic rosmsg 等ros命令的重要性：

先列出常见常用的ros命令：

$ roscd [功能包名称] //移动到保存有功能包的目录
$ rosls [功能包名称] //查看指定的ROS功能包的文件列表 

$ rostopic list: 显示当前正在发送和接收的所有话题的列表 
$ rostopic echo [话题名称]: 实时显示指定话题的消息内容
$ rostopic find [类型名称]: 显示使用指定类型的消息的话题 
$ rostopic type [话题名称]: 显示指定话题的消息类型 
rosservice rosparam 也是一样的使用，有一点点区别；service对应服务通信机制，topic对应话题（发布者订阅者）机制，Param对应参数服务器通信机制；

$ rosmsg list // 显示所有消息
$ rosmsg show [消息名称] // 显示指定消息

$ rosbag record [选项][话题名称]:记录指定话题的消息
    -a表示录制所有话题;-o [filename]表示输出到filename的bag文件;例:rosbag record -a -o all表示将录制all_xxx.bag的bag文件到当前目录，xxx是自动生成的日期序列；
$ rosbag info [bag文件名]:查看bag文件的信息 
$ rosbag play [bag文件名]:回放指定的bag文件 

$ rospack //显示与ROS功能包相关的信息

rosclean：检查或删除ROS日志文件
$ rosclean check        ->检查命令
$ rosclean purge        ->删除命令

$ rosnode list //查看活动的节点列表
$ rosnode info [节点名称]

再回到我们的任务来：首先我们需要录制一段会发布 "/odom" 话题的bag包（为什么是/odom话题，这个就得自己摸索着看每个话题的消息类型是不是你想要订阅的消息）,这里我们小车启动雷达会发布该里程计话题，所以我们启动雷达，录制一段bag包保存为odom.bag：

使用rosbag play odom.bag来进行包的回放：

回放的过程中我们使用rostopic list 查看当前的话题：

这里就验证了在/odom话题中使用的消息类型是nav_msgs/Odometry ，该消息中包含了我们需要的linear angular 线速度和角速度；我们接下来就订阅这个消息，编写订阅者程序（这里在ros创建工作空间，初始化等工作就不演示了）：

#include "ros/ros.h"
// #include "sensor_msgs/Imu.h"
#include "nav_msgs/Odometry.h"

void doPose(const nav_msgs::Odometry::ConstPtr& p){
    ROS_INFO("linear.x=%.2f,angular.z=%.2f",
        p->twist.twist.linear.x,p->twist.twist.angular.z
    );
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    // 2.初始化 ROS 节点
    ros::init(argc,argv,"sub_pose");
    // 3.创建 ROS 句柄
    ros::NodeHandle nh;
    // 4.创建订阅者对象
    ros::Subscriber sub = nh.subscribe<nav_msgs::Odometry>("/odom",1000,doPose);
    // 5.回调函数处理订阅的数据
    // 6.spin
    ros::spin();
    return 0;
}

注意在CMakeLists.txt中完成nav_msgs依赖导入，然后完成相关配置；运行编译，rosrun该订阅节点，如下：

由此完成该问题；该任务主要是达到熟练编写理解订阅者程序、掌握ros相关常用命令、以及熟悉我们实验的小车。

小结：对于我这种小白来说需要这样的复盘来回顾我做的任务背后的含义，其中的代码也需要进一步理解，有错误望指正。另外对于小车的各部分结构及之间的通信还需要了解熟悉，时常感觉不清楚为什么该消息类型中会有这样那样的数据，这些数据是怎么来的，是根据其他传感器元件的数据计算而来还是自身的传感器自带的，这些数据得来后在哪里又需要用到它？这些问题虽然查过，但现阶段还是没有形成体系，后面和队友继续加油！

OpenCV显示深度相机的RGB图像和深度图

1、同样先打开深度相机；

2、查看需要订阅的深度相机话题：

这里我们事先录制了一个开启小车所有传感器下的all.bag；运行查看需要订阅的话题，根据话题名称可以大概的知道需要订阅的是/camera/color/image_raw 和 /camera/depth/image_rect_raw，分别对应任务要求的rgb彩色图像和深度图像；程序如下（借鉴的官网程序cv_bridge/Tutorials/UsingCvBridgeToConvertBetweenROSImagesAndOpenCVImages - ROS Wiki）：

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
 
static const std::string OPENCV_WINDOW_COLOR = "Image color window";
static const std::string OPENCV_WINDOW_DEPTH = "Image depth window";
 
class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_color;
  image_transport::Subscriber image_sub_depth;
  image_transport::Publisher image_pub_color;
  image_transport::Publisher image_pub_depth;
 
public:
  ImageConverter()
    : it_(nh_)
  {
    // Subscrive to input video feed and publish output video feed
    image_sub_color = it_.subscribe("/camera/color/image_raw", 1, &ImageConverter::imageCb, this);
    image_sub_depth = it_.subscribe("/camera/depth/image_rect_raw",1,&ImageConverter::depthCb, this);
    image_pub_color = it_.advertise("/image_converter/output_video/color", 1);
    image_pub_depth = it_.advertise("/image_converter/output_video/depth", 1);

 
    cv::namedWindow(OPENCV_WINDOW_COLOR);
    cv::namedWindow(OPENCV_WINDOW_DEPTH);
  }
 
  ~ImageConverter()
  {
    cv::destroyWindow(OPENCV_WINDOW_COLOR);
    cv::destroyWindow(OPENCV_WINDOW_DEPTH);
  }
 
  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
 
    // Draw an example circle on the video stream
    if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
      cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));
 
    // Update GUI Window
    cv::imshow(OPENCV_WINDOW_COLOR, cv_ptr->image);
    cv::waitKey(3);
 
    // Output modified video stream
    image_pub_color.publish(cv_ptr->toImageMsg());
  }

  void depthCb(const sensor_msgs::ImageConstPtr &msg)
  {
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
      cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }

     // Draw an example circle on the video stream
    if (cv_ptr->image.rows > 60 && cv_ptr->image.cols > 60)
      cv::circle(cv_ptr->image, cv::Point(50, 50), 10, CV_RGB(255,0,0));

    // Update GUI Window
    cv::imshow(OPENCV_WINDOW_DEPTH, cv_ptr->image);
    cv::waitKey(3);
 
    // Output modified video stream
    image_pub_depth.publish(cv_ptr->toImageMsg());

  }
};
 
int main(int argc, char** argv)
{
  ros::init(argc, argv, "image_converter");
  ImageConverter ic;
  ros::spin();
  return 0;
}

阅读教程后修改自己要订阅的节点。效果如下（深度图像有的也是黑色，这里有疑惑的地方，待会查查）：

小结：多学多练。。。把代码功能，原理搞懂。。。

PCL显示激光雷达的点云数据

参考：(16条消息) 使用ros订阅点云并在pcl::visualization中实时显示_拔萝卜的小白TWO的博客-CSDN博客_ros订阅点云数据

#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <string.h>
#include <pcl/io/pcd_io.h>
// #include <pcl/point_types.h>
#include <pcl/io/io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
// #include <pcl/filters/statistical_outlier_removal.h>   //统计滤波
// #include <pcl/filters/random_sample.h>    //随机采样
// #include <pcl/filters/voxel_grid.h>      //体素滤波
// #include <pcl/filters/passthrough.h>     //直通滤波
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

#include <pcl_conversions/pcl_conversions.h>
using namespace std;
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer1(new pcl::visualization::PCLVisualizer("realtime pcl"));

ros::Publisher pub;

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
   // 声明存储原始数据与滤波后的数据的点云的格式
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;    //原始的点云的数据格式
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  // 转化为PCL中的点云的数据格式
  pcl_conversions::toPCL(*input, *cloud);

  pub.publish (*cloud);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1;
  cloud1.reset (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromROSMsg (*input, *cloud1);

//  pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
//  viewer.showCloud(cloud1,"Simple Cloud Viewer");

  viewer1->removeAllPointClouds();  // 移除当前所有点云
  viewer1->addPointCloud(cloud1, "realtime pcl");
  viewer1->updatePointCloud(cloud1, "realtime pcl");
  viewer1->spinOnce(0.001);

}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "pcl");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  //"/camera/depth/color/points"realsense发布的点云数据
  ros::Subscriber sub = nh.subscribe ("/rslidar_points", 10, cloud_cb);
  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<pcl::PCLPointCloud2> ("output", 10);
  // Spin
  ros::spin ();
}

效果：

小结：自己找代码方法和编程能力都亟待提高，背后还有这些实验的原理。以上真实记录了自己完成任务的过程，目前的博客都是自己的一些记录和问题分析，方便自己回顾。

任务二：

编程实现用里程计（odometry）计算小车移动距离

小车靜止不动，读取里程计数据，记为a控制小车前进n米距离(n=1、2、3均可），读取里程计数据，记为b
建立小车移动距离与里程计读数a、b之间的关系模型（前两步应进行多次以拟合更精确的模型）
控制小车移动，利用上一步构建的模型，计算小车移动的距离，并将计算结果与实际值进行对比

实现逻辑：订阅里程计里面的线速度，针对线速度波动进行一定的修正，然后根据发布频率来实现积分中的时间间隔，进行积分，得到距离；代码：

#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <math.h>
float distance = 0.0;
void doMsg(const nav_msgs::Odometry::ConstPtr& msg)
{
    float speed_x=0;    
    speed_x = msg->twist.twist.linear.x;    
    if(fabs(speed_x)<0.05)
    {
        speed_x=0;
    }
    ROS_INFO("X_速度：%.3f ", speed_x);
    distance += 0.1 * speed_x;
    ROS_INFO("里程计的距离为：%.6f",distance );
}
int main(int argc, char  *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "odom_sub");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("/odom",1000,doMsg);
    ros::spin();
    return 0;
}

效果（下图是之前的程序跑的，之前的代码中把y,z的线速度也订阅了，然后进行三个方向的平方和求总速度，但是这样由于静止时速度会有波动（为什么呢？它的测速原理是什么呢？还有y,z方向的速度一直都为0,没有波动是为什么呢？在寻找了一番后发现里程计的速度是根据imu的加速度算的（这里可能得看源代码再次确认一下），但是还是不太理解为什么会波动？？？），导致我们在静止时也会不断的增加里程计读数；所以我们进行了修改，就直接订阅x轴的速度，以为它自身正负的波动会自动抵消，然鹅并没有什么用，还是会增加，所以我们又进一步直接粗暴的设置一个误差下限，假设速度小于0.05的都是误差不计，这样在短距离里面比较准，同时静止时也不会存在里程计增加的情况，但是在远距离下误差会很大，不同测次误差都很大，有时候接近实际值，有时候又偏离很远，这一部分的原因也还不是很清楚？？？后继继续搞懂）：文章来源地址https://uudwc.com/A/vm3PG

车载软件开发基础

命令行窗口显示小车的线速度和角速度

1、开启各类传感器：

2、查找需要订阅的线速度和角速度的节点或话题是什么

OpenCV显示深度相机的RGB图像和深度图

PCL显示激光雷达的点云数据

任务二：