初步调试走迷宫算法完成

src/CMakeLists.txt

@@ -1 +1 @@
-/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
+/opt/ros/kinetic/catkin/cmake/toplevel.cmake

src/infrared_maze/cfg/param.yaml

@@ -5,11 +5,11 @@
 # Author: corvin
 # History:
 #   20200404:init this file.
-#   20200406:增加yaw_data话题,rate参数.
+#   20200406:增加yaw service,rate参数.
 ######################################################
 infrared_topic: /mobilebase_arduino/sensor/GP2Y0A41
 cmd_topic: /cmd_vel
-yaw_topic: /yaw_data
+yaw_service: /imu_node/GetYawData
 linear_x: 0.17
 linear_y: 0.15
 angular_speed: 0.5

src/infrared_maze/src/infrared_maze.cpp

@@ -6,20 +6,24 @@
     20200404:增加可以直接转90度,掉头的功能 by corvin.
  **************************************************************/
 #include <ros/ros.h>
-#include "ros_arduino_msgs/InfraredSensors.h"
+#include <std_msgs/Float32.h>
+#include <nav_msgs/Odometry.h>
 #include <geometry_msgs/Twist.h>
 #include <tf/transform_listener.h>
-#include <nav_msgs/Odometry.h>
-#include <std_msgs/Float32.h>
+#include "rasp_imu_hat_6dof/GetYawData.h"
+#include "ros_arduino_msgs/InfraredSensors.h"
 
 
 #define  GO_FORWARD   0
 #define  TURN_LEFT    1
 #define  TURN_RIGHT   2
 #define  TURN_BACK    3
+#define  ERROR       -1
 
 //global variable
 ros::Publisher twist_pub;
+ros::ServiceClient srvClient;
+rasp_imu_hat_6dof::GetYawData srv;
 
 float warn_dist = 0.15;  //warn check distance
 float tolerance_dist = 0.04; //对警告距离的容忍值
@@ -35,9 +39,8 @@ float linear_x_speed = 0.17;
 float linear_y_speed = 0.15;
 float angular_speed = 0.5;
 
-float now_yaw_data = 0.0;
 float start_yaw_data = 0.0;
-int turnFlag = 0;
+int turnFlag = ERROR;
 
 /*********************************************************
  * Descripition: 发布控制移动的消息,这里对于平面移动的
@@ -112,9 +115,10 @@ void goForward()
     float y_start = tf_Transform.getOrigin().y();
     while((dist < check_dist) && (ros::ok()))
     {
-        ROS_INFO("Go forward ...");
+        ROS_INFO("Go forward, dist:%f", dist);
         publishTwistCmd(linear_x_speed, 0, 0);
         ros::Duration(0.10).sleep(); //100 ms
+
         tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0),tf_Transform);
         float x = tf_Transform.getOrigin().x();
         float y = tf_Transform.getOrigin().y();
@@ -122,12 +126,13 @@ void goForward()
     }
     publishTwistCmd(0, 0, 0); //stop move
     ROS_INFO("Go forward finish !!!");
-    ros::Duration(0.3).sleep(); //300 ms
+    ros::Duration(0.1).sleep(); //100 ms
 }
 
-/**************************************************
- * Description:控制小车左右转90度,或者掉头180度.
- *************************************************/
+/*************************************************************
+ * Description:控制小车左右转90度,或者掉头180度.这里实现小车
+ *   转弯功能是通过不断监听odom和base_link之间的tf转换完成的.
+ ************************************************************/
 int controlTurn(int flag)
 {
     tf::TransformListener tf_Listener;
@@ -149,12 +154,16 @@ int controlTurn(int flag)
             check_angle = M_PI/2.0;
             break;
 
-        case TURN_BACK: //小车原地掉头
+        case TURN_BACK: //小车原地掉头180度
             turnFlag = TURN_BACK;
             check_angle = M_PI;
             angular = angular_speed;
             break;
 
+        case GO_FORWARD: //小车直行50cm
+            goForward();
+            return 1;
+
         default:
             ROS_ERROR("Turn flag error !");
             return -1;
@@ -167,8 +176,9 @@ int controlTurn(int flag)
     catch(tf::TransformException &ex)
     {
         ROS_ERROR("tf_Listener.waitForTransform timeout error !");
-        ROS_ERROR("%s",ex.what());
+        ROS_ERROR("%s", ex.what());
         ros::shutdown();
+
         return -2;
     }
 
@@ -176,10 +186,9 @@ int controlTurn(int flag)
     tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
     float last_angle = fabs(tf::getYaw(tf_Transform.getRotation()));
     float turn_angle = 0.0;
-    //ROS_INFO("first_angle:%f",last_angle);
+    ROS_INFO("first_angle:%f",last_angle);
     while((fabs(turn_angle) < check_angle)&&(ros::ok()))
     {
-        ROS_INFO("Turning ...");
         publishTwistCmd(0, 0, angular); //原地转弯,不需要linear_x,y的控制
         ros::Duration(0.10).sleep(); //100 ms
 
@@ -197,134 +206,131 @@ int controlTurn(int flag)
 
         turn_angle += delta_angle;
         last_angle = rotation;
-        //ROS_INFO("turn_angle:%f",turn_angle);
+        ROS_INFO("Turn angle:%f", turn_angle);
     }
-    publishTwistCmd(0, 0, 0); //原地转弯,不需要linear_x,y的控制
+    publishTwistCmd(0, 0, 0); //原地转弯完成后,需要立刻停止小车的旋转
     ROS_INFO("Turning finish !!!");
-    ros::Duration(0.3).sleep(); //300 ms
+    ros::Duration(0.1).sleep(); //100 ms
 
     return 0;
 }
 
 /***************************************************************************
  * Description:检查三个传感器数据,然后控制小车移动.这里需要防止小车在原地
- *   进行连续两次旋转,在每次转弯后必须有个直行.
+ *   进行连续两次旋转,在每次转弯后必须有个直行.这里需要注意的是当小车在转弯
+ *   后一定要有一个可以修正转弯角度的功能,否则转弯后角度不准确小车前进方向
+ *   会错.这里的走迷宫策略是左转优先,中间是直行,最后是右转.
  **************************************************************************/
 void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
         float tolerance)
 {
     static int flag = 0;
-    ros::Duration(0.5).sleep();
 
+    srvClient.call(srv); //通过服务调用来获取yaw值
+    start_yaw_data = srv.response.yaw; //记录当前yaw值,用于后面修正
+
+    //判断能否左转,并且上一次动作不能是转弯
     if((infrared_l == (float)0.3)&&(flag != 1))
     {
-        start_yaw_data = now_yaw_data;
         ROS_WARN("turn Left! yaw_data:%f", start_yaw_data);
-        controlTurn(TURN_LEFT);
         turnFlag = TURN_LEFT;
         flag = 1;
     }
-    else if(infrared_f > tolerance*5.0)
+    else if(infrared_f > tolerance*5.0)//判断能否直行
     {
         //始终控制小车保持在过道的中间位置
+        ROS_WARN("go Forward ! yaw_data: %f", start_yaw_data);
         horizMoveMiddle(infrared_l, infrared_r, tolerance);
-        //publishTwistCmd(linear_x_speed, 0, 0); //go forward
-        goForward();
-        flag = 0;
         turnFlag = GO_FORWARD;
+        flag = 0;
     }
-    else if((infrared_r == (float)0.3)&&(flag != 1))
+    else if((infrared_r == (float)0.3)&&(flag != 1))//判断能否右转
     {
-        start_yaw_data = now_yaw_data;
-        ROS_WARN("turn Right!, yaw_data: %f", start_yaw_data);
-        controlTurn(TURN_RIGHT);
+        ROS_WARN("turn Right ! yaw_data: %f", start_yaw_data);
         turnFlag = TURN_RIGHT;
         flag = 1;
     }
-    else
+    else //小车掉头
     {
-        start_yaw_data = now_yaw_data;
-        ROS_WARN("turn Back! yaw_data: %f", start_yaw_data);
-        controlTurn(TURN_BACK);
+        ROS_WARN("turn Back ! yaw_data: %f", start_yaw_data);
         turnFlag = TURN_BACK;
     }
+
+    controlTurn(turnFlag); //开始执行动作
 }
 
-void yaw_callback(const std_msgs::Float32::ConstPtr& msg)
+/**********************************************************
+ * Description:调用IMU板发布出来的yaw信息,可以根据该数据
+ *   来修正转弯时不准确的问题.注意这里的yaw数据为弧度值.
+ *   注意往右旋转为负值,左旋转为正值.右手定则.
+ *   注意在ROS中IMU数据中yaw的范围,具体表示如下:
+ *                0  -0
+ *                -----
+ *              /      \
+ *         1.57 |      | -1.57
+ *              \      /
+ *               ------
+ *             3.14 -3.14
+ **********************************************************/
+void yawUpdate(const float start_yaw)
 {
-    ROS_INFO("Yaw data:%f", msg->data);
-    now_yaw_data = msg->data;
+    float target = 0.0;
+    float diff = 10.0;
+    float angular = 0.0;
+    float now_yaw = 0.0;
 
-    if(turnFlag != GO_FORWARD) //开始纠正转弯的角度
+    //开始计算修正后的目标角度
+    if(turnFlag == GO_FORWARD)
     {
-        float target = 0.0;
-        float diff = 0.0;
-        float sleep_time = 0.0;
-        float angular = 0.0;
-
-        if(turnFlag == TURN_LEFT)
+        target = start_yaw;
+    }
+    else if(turnFlag == TURN_LEFT)
+    {
+        target = start_yaw + M_PI/2.0;
+        if(target > M_PI)
         {
-            target = start_yaw_data + M_PI/2.0;
-            if(target > M_PI)
-            {
-                target = target - M_PI*2.0;
-            }
-            ROS_INFO("Target Yaw: %f", target);
-            diff = target - now_yaw_data;
-            sleep_time = fabs(diff)/angular_speed;
-            if(diff > 0)
-            {
-                angular = angular_speed;
-            }
-            else
-            {
-                angular = -angular_speed;
-            }
+            target = target - M_PI*2.0;
         }
-        else if(turnFlag == TURN_RIGHT)
+    }
+    else if(turnFlag == TURN_RIGHT)
+    {
+        target = start_yaw - M_PI/2.0;
+        if(target < -M_PI)
         {
-            target = start_yaw_data - M_PI/2.0;
-            if(target < -M_PI)
-            {
-                target = target + M_PI*2.0;
-            }
-            ROS_INFO("Target Yaw: %f", target);
-            diff = target - now_yaw_data;
-            sleep_time = fabs(diff)/angular_speed;
-            if(diff > 0)
-            {
-                angular = angular_speed;
-            }
-            else
-            {
-                angular = -angular_speed;
-            }
+            target = target + M_PI*2.0;
         }
-        else //turn back
+    }
+    else //turn back
+    {
+        target = start_yaw + M_PI;
+        if(target > M_PI)
         {
-            target = start_yaw_data + M_PI;
-            if(target > M_PI)
-            {
-                target = target - M_PI*2.0;
-            }
-            ROS_INFO("Target Yaw: %f", target);
-            diff = target - now_yaw_data;
-            sleep_time = fabs(diff)/angular_speed;
-            if(diff > 0)
-            {
-                angular = angular_speed;
-            }
-            else
-            {
-                angular = -angular_speed;
-            }
+            target = target - M_PI*2.0;
         }
-        ROS_WARN("Yaw diff: %f, sleep:%f", diff, sleep_time*2.0);
-        publishTwistCmd(0, 0, angular*2);
-        ros::Duration(sleep_time*2.0).sleep();
-        ROS_WARN("Correct Yaw diff over !");
-        publishTwistCmd(0, 0, 0);
     }
+
+    //开始不断的计算偏差,调整角度
+    while(fabs(diff) > 0.01)
+    {
+        srvClient.call(srv);
+        now_yaw = srv.response.yaw;
+        ROS_INFO("Now Yaw data:%f, Target Yaw:%f", now_yaw, target);
+
+        diff = target - now_yaw;
+        if(diff > 0)
+        {
+            angular = angular_speed;
+        }
+        else
+        {
+            angular = -angular_speed;
+        }
+        ROS_WARN("Yaw diff:%f, angular:%f", diff, angular);
+        publishTwistCmd(0, 0, angular);
+        ros::Duration(0.04).sleep();
+    }
+    publishTwistCmd(0, 0, 0); //在修正角度结束后应该立刻停止小车转动
+    ROS_INFO("Correct Yaw diff over !");
 }
 
 /*****************************************************************************
@@ -341,6 +347,7 @@ void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
 
     //根据三个传感器反馈的测距值开始移动
     checkInfraredDist(front_dist, left_dist, right_dist, tolerance_dist);
+    yawUpdate(start_yaw_data); //移动完成后开始更新yaw值
 }
 
 /************************************************************
@@ -352,26 +359,27 @@ int main(int argc, char **argv)
 {
     std::string infrared_topic;
     std::string cmd_topic;
-    std::string yaw_topic;
+    std::string yaw_service;
 
     ros::init(argc, argv, "infrared_maze_node");
     ros::NodeHandle handle;
 
     ros::param::get("~infrared_topic", infrared_topic);
     ros::param::get("~cmd_topic", cmd_topic);
-    ros::param::get("~linear_x", linear_x_speed);
-    ros::param::get("~linear_y", linear_y_speed);
+    ros::param::get("~yaw_service", yaw_service);
+    ros::param::get("~linear_x",  linear_x_speed);
+    ros::param::get("~linear_y",  linear_y_speed);
     ros::param::get("~angular_speed", angular_speed);
     ros::param::get("~warn_dist", warn_dist);
     ros::param::get("~tolerance_dist", tolerance_dist);
     ros::param::get("~odom_frame", odomFrame);
     ros::param::get("~base_frame", baseFrame);
-    ros::param::get("~yaw_topic",  yaw_topic);
 
     ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 1, infrared_callback);
-    ros::Subscriber sub_yaw = handle.subscribe(yaw_topic, 1, yaw_callback);
     twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
+    srvClient = handle.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
 
     ros::spin();
     return 0;
 }
+

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/CMakeLists.txt

@@ -3,14 +3,30 @@ project(rasp_imu_hat_6dof)
 
 find_package(catkin REQUIRED COMPONENTS 
     rospy
-    dynamic_reconfigure)
+    std_msgs
+    dynamic_reconfigure
+    message_generation)
+
+add_service_files(
+    FILES
+    GetYawData.srv
+)
+
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+)
 
 ## Generate dynamic reconfigure parameters in the 'cfg' folder
 generate_dynamic_reconfigure_options(
     cfg/imu.cfg
 )
 
-catkin_package()
+catkin_package(
+    CATKIN_DEPENDS
+    std_msgs
+    message_runtime
+)
 
 include_directories(
     ${catkin_INCLUDE_DIRS}
@@ -32,7 +48,8 @@ install(DIRECTORY src
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
-catkin_install_python(PROGRAMS 
+catkin_install_python(PROGRAMS
 	nodes/imu_node.py
     DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/nodes
 )
+

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/nodes/imu_node.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 # -*- coding: UTF-8 -*-
 
-# Copyright: 2016-2019 https://www.corvin.cn ROS小课堂
+# Copyright: 2016-2020 https://www.corvin.cn ROS小课堂
 # Author: corvin
 # Description: 为树莓派IMU扩展板所使用的配套代码，由于默认
 #    扩展板与树莓派使用IIC连接。所以这里的代码是直接从IIC接口
@@ -9,12 +9,14 @@
 #    中的IMU消息格式，发布到/imu话题中，这样有需要的节点可以
 #    直接订阅该话题即可获取到imu扩展板当前的数据。
 # History:
-#    20191031: Initial this file.
-#    20191209: 新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
-#    20200406: 增加可以直接发布yaw数据的话题.
+#    20191031:Initial this file.
+#    20191209:新增发布IMU芯片温度的话题，发布频率与发布imu数据频率相同.
+#    20200406:增加可以直接发布yaw数据的话题.
+#    20200410:增加通过service方式发布yaw数据,方便其他节点调用.
 import rospy
 import math
 
+from rasp_imu_hat_6dof.srv import GetYawData,GetYawDataResponse
 from tf.transformations import quaternion_from_euler
 from dynamic_reconfigure.server import Server
 from rasp_imu_hat_6dof.cfg import imuConfig
@@ -25,6 +27,11 @@ from imu_data import MyIMU
 
 degrees2rad = math.pi/180.0
 imu_yaw_calibration = 0.0
+yaw = 0.0
+
+# Server return Yaw data
+def return_yaw_data(req):
+    return GetYawDataResponse(yaw)
 
 # Callback for dynamic reconfigure requests
 def reconfig_callback(config, level):
@@ -46,6 +53,7 @@ data_pub = rospy.Publisher(data_topic_name, Imu, queue_size=1)
 temp_pub = rospy.Publisher(temp_topic_name, Float32, queue_size=1)
 yaw_pub = rospy.Publisher(yaw_topic_name, Float32, queue_size=1)
 srv = Server(imuConfig, reconfig_callback)  # define dynamic_reconfigure callback
+yaw_srv = rospy.Service('imu_node/GetYawData', GetYawData, return_yaw_data)
 
 imuMsg = Imu()
 yawMsg = Float32()

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/package.xml

@@ -15,11 +15,15 @@
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>std_msgs</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>message_generation</build_depend>
 
   <run_depend>rospy</run_depend>
   <run_depend>tf</run_depend>
   <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>message_runtime</run_depend>
 </package>
 

src/rasp_imu_hat_6dof/rasp_imu_hat_6dof/srv/GetYawData.srv

@@ -0,0 +1,2 @@
+---
+float32 yaw