更新robot_demo中颜色跟踪和imu_demo的launch文件,启动时先将robot_bringup启动

src/robot_demo/launch/color_demo.launch

@@ -5,8 +5,10 @@
  * @History: 20210430:初始化文件-adam_zhuo
 -->
 <launch>
-  <!-- 启动color_detection_node -->
-  <node pkg="robot_demo" type="robot_demo_color_node" name="robot_demo_color_node" output="screen">
+  <!-- launch robot_bringup.launch -->
+  <include file="$(find robot_bringup)/launch/robot_bringup.launch" />
 
+  <!-- run color_detection_node -->
+  <node pkg="robot_demo" type="robot_demo_color_node" name="robot_demo_color_node" output="screen">
   </node>
-</launch>
+</launch>

src/robot_demo/launch/imu_demo.launch

@@ -5,7 +5,10 @@
  * @History: 20210512:初始化文件-adam_zhuo
 -->
 <launch>
-  <!-- 启动robot_demo_imu_node -->
+  <!-- launch robot_bringup.launch -->
+  <include file="$(find robot_bringup)/launch/robot_bringup.launch" />
+
+  <!-- run robot_demo_imu_node -->
   <node pkg="robot_demo" type="robot_demo_imu_node" name="robot_demo_imu_node" output="screen">
   </node>
 </launch>

src/robot_demo/src/color_detection.cpp

@@ -6,25 +6,23 @@
  */
 #include "color_detection.h"
 
-Color_Detection::Color_Detection(){
-
+Color_Detection::Color_Detection()
+{
     //订阅者订阅压缩图像
     _img_sub = nh.subscribe<sensor_msgs::CompressedImage>("/raspicam_node/image/compressed", 1,
                                                          &Color_Detection::img_callback, this);
     //发布者发布速度消息
     _cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
-    
 }
 
 Color_Detection::~Color_Detection(){
-
 }
 
 /**
  * @description: 订阅图像数据回调函数
  */
-void Color_Detection::img_callback(const sensor_msgs::CompressedImage::ConstPtr & data){
-
+void Color_Detection::img_callback(const sensor_msgs::CompressedImage::ConstPtr & data)
+{
     cv_bridge::CvImagePtr cv_ptr;
     //将ROS中图像数据转化成OpenCV-BRG8数据
     cv_ptr = cv_bridge::toCvCopy(data, sensor_msgs::image_encodings::BGR8);
@@ -32,13 +30,14 @@ void Color_Detection::img_callback(const sensor_msgs::CompressedImage::ConstPtr
     cv::Mat img, img_copy;
     cv_ptr->image.copyTo(img);
     cv_ptr->image.copyTo(img_copy);
-    
+
     double prec_tick = ticks;
     ticks = (double)cv::getTickCount();
 
     double dT = (ticks - prec_tick) / cv::getTickFrequency();
-    
-    if(found){
+
+    if(found)
+    {
         //卡尔曼滤波预测跟踪小球
         img_kalman_filter.found_draw(dT, img_copy, x, area);
         //根据小球与小车的位置关系发布小车的速度
@@ -81,13 +80,16 @@ void Color_Detection::img_callback(const sensor_msgs::CompressedImage::ConstPtr
     cout << "Balls found:" << balls_box.size() << endl;
     // <<<<< OpenCV处理图像找出画面中蓝色小球
 
-    if(balls.size() == 0){
+    if(balls.size() == 0)
+    {
         not_found_count++;
         cout << "notFoundCount:" << not_found_count << endl;
         if(not_found_count >= 100 ){
             found = false;
         }
-    }else{
+    }
+    else
+    {
         not_found_count = 0;
         //卡尔曼滤波更新
         img_kalman_filter.kalman_update(found, balls_box);
@@ -96,14 +98,14 @@ void Color_Detection::img_callback(const sensor_msgs::CompressedImage::ConstPtr
     //显示图像
     cv::imshow("color_detection", img_copy);
     cv::waitKey(1);
-    
 }
 
 /**
  * @description: 根据小球与小车位置控制小车移动，当小球不在小车中心时控制小车旋转，
  *               当小球距小车较远时小车前进，当小球距小车较近时小扯后退
  */
-void Color_Detection::cmd_vel_pub(float x, float area){
+void Color_Detection::cmd_vel_pub(float x, float area)
+{
     _twist.linear.x = 0;
     _twist.angular.z = 0;
 
@@ -127,7 +129,6 @@ void Color_Detection::cmd_vel_pub(float x, float area){
     if(abs(_twist.linear.x) < 0.015){
         _twist.linear.x = 0;
     }
-    
-    _cmd_vel_pub.publish(_twist);
 
-}
+    _cmd_vel_pub.publish(_twist);
+}

src/robot_demo/src/color_detection_node.cpp

@@ -6,12 +6,11 @@
  */
 #include "color_detection.h"
 
-int main(int argc, char **argv){
-
-    //初始化节点
+int main(int argc, char **argv)
+{
     ros::init(argc, argv, "color_detection_node");
     //根据Color_Detection声明对象，调用构造函数
     Color_Detection color_detection;
     //使ROS中回调函数生效
     ros::spin();
-}
+}

src/robot_demo/src/imu_demo_node.cpp

@@ -9,21 +9,22 @@
 /**
  * @description: 构造函数初始化变量。
  */
-Imu_Demo::Imu_Demo(){
+Imu_Demo::Imu_Demo()
+{
     start_flag = false;
-    _angle_sub = nh.subscribe<std_msgs::Float32>("/angle", 1, &Imu_Demo::angle_callback, this);
+    _angle_sub = nh.subscribe<std_msgs::Float32>("/fixed_angle", 1, &Imu_Demo::angle_callback, this);
     _yaw_sub = nh.subscribe<std_msgs::Float32>("/yaw_data", 1, &Imu_Demo::yaw_callback, this);
     _cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
 }
 
 Imu_Demo::~Imu_Demo(){
-
 }
 
 /**
  * @description: 接收指定角度回调函数。程序启动后,发送指定角度，任意放置小车，小车最终朝向都会是指定角度。
  */
-void Imu_Demo::angle_callback(const std_msgs::Float32::ConstPtr &data){
+void Imu_Demo::angle_callback(const std_msgs::Float32::ConstPtr &data)
+{
     start_flag = true;
     target_radian = data->data/180*M_PI;
     target_radian_copy = target_radian;
@@ -34,8 +35,8 @@ void Imu_Demo::angle_callback(const std_msgs::Float32::ConstPtr &data){
 /**
  * @description: yaw角回调函数，不断检测当前小车朝向与指定角度的差值，如果大于一个阈值，则控制小车旋转。
  */
-void Imu_Demo::yaw_callback(const std_msgs::Float32::ConstPtr &data){
-
+void Imu_Demo::yaw_callback(const std_msgs::Float32::ConstPtr &data)
+{
     //如果没有接收到发送的指定角度，则不做任何处理直接return
     if(!start_flag)
         return;
@@ -68,7 +69,6 @@ void Imu_Demo::yaw_callback(const std_msgs::Float32::ConstPtr &data){
 
 int main(int argc, char **argv)
 {
-    //初始化节点
     ros::init(argc, argv, "imu_demo_node");
 
     //根据Imu_Demo声明对象，调用构造函数

update_code.sh

@@ -23,6 +23,7 @@ echo -e "\n"
 
 sleep 3
 echo -e "${green}>>> 2: 开始编译代码${normal}"
+rm -rf .ros/log/
 catkin_make
 
 cd src/dual_iic/build