更新使用三个红外测据传感器走迷宫的代码

src/infrared_maze/CMakeLists.txt

@@ -11,6 +11,8 @@ find_package(catkin REQUIRED COMPONENTS
   ros_arduino_msgs
   roscpp
   geometry_msgs
+  nav_msgs
+  tf
 )
 
 ## System dependencies are found with CMake's conventions
@@ -116,8 +118,8 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
-# include
-  ${catkin_INCLUDE_DIRS}
+ include
+ ${catkin_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library

src/infrared_maze/cfg/param.yaml

@@ -0,0 +1,14 @@
+#####################################################
+# Copyright: 2016-2020 www.corvin.cn ROS小课堂
+# Description:使用小车上三个红外测距传感器进行避障
+#   走迷宫,这里是一些可以配置的参数.
+# Author: corvin
+# History:
+#   20200404:init this file.
+######################################################
+infrared_topic: /mobilebase_arduino/sensor/GP2Y0A41
+cmd_topic: /cmd_vel
+linear_speed: 0.15
+angular_speed: 0.4
+odom_frame: /odom_combined
+base_frame: /base_footprint

src/infrared_maze/launch/infrared_maze.launch

@@ -1,13 +1,16 @@
 <!--
-  Copyright: 2016-2018 www.corvin.cn
-  Author: jally
+  Copyright: 2016-2020 www.corvin.cn
+  Author: jally, corvin
   Description:
     maze with infrared
   History:
     20200325: intial this file.
 -->
 <launch>
-  <!-- startup ultrasonic_obstacle_avoidance_node -->
-  <node pkg="infrared_maze" type="infrared_maze_node" name="infrared_maze_node" output="screen"/>
-</launch>
+  <arg name="cfg_file" default="$(find infrared_maze)/cfg/param.yaml" />
 
+  <!-- startup infrared_obstacle_avoidance_node -->
+  <node pkg="infrared_maze" type="infrared_maze_node" name="infrared_maze_node" output="screen">
+    <rosparam file="$(arg cfg_file)" command="load" />
+  </node>
+</launch>

src/infrared_maze/package.xml

@@ -7,7 +7,7 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="jally_zhang@corvin.cn">tianbot</maintainer>
+  <maintainer email="jally_zhang@corvin.cn">jally</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
@@ -51,10 +51,12 @@
   <buildtool_depend>catkin</buildtool_depend>
   <build_depend>ros_arduino_msgs</build_depend>
   <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
   <build_export_depend>ros_arduino_msgs</build_export_depend>
   <build_export_depend>roscpp</build_export_depend>
   <exec_depend>ros_arduino_msgs</exec_depend>
   <exec_depend>roscpp</exec_depend>
+  <exec_depend>tf</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

src/infrared_maze/src/infrared_maze (copy).cpp

@@ -1,31 +0,0 @@
-/**************************************************************
- *Copyright(C): ROS小课堂 www.corvin.cn
- *FileName: getdata.cpp
- *Author: jally
- *Date: 20200325
- *Description:获取红外数据
- *
- **************************************************************/
-#include "ros/ros.h"
-#include "ros_arduino_msgs/InfraredSensors.h"
- 
- 
-void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
-{
-    ROS_INFO("front distance:[%f]", msg->front_dist);
-    ROS_INFO("left distance:[%f]", msg->left_dist);
-    ROS_INFO("right distance:[%f]", msg->right_dist);
-}
- 
- 
-int main(int argc, char **argv)
-{
-    ros::init(argc, argv, "infrared_maze_node");
-    ros::NodeHandle handle;
- 
-    ros::Subscriber sub_infrared = handle.subscribe("/mobilebase_arduino/sensor/GP2Y0A41", 100, infrared_callback);
- 
-    ros::spin();
- 
-    return 0;
-}

src/infrared_maze/src/infrared_maze.cpp

@@ -1,15 +1,17 @@
 /**************************************************************
- *Copyright(C): ROS小课堂 www.corvin.cn
- *FileName: getdata.cpp
- *Author: jally
- *Date: 20200325
- *Description:三轮小车进行红外避障
- *
+ Copyright(C): 2016-2020 ROS小课堂 www.corvin.cn
+ Description:三轮小车进行红外避障走迷宫.
+ Author: jally, corvin
+ History:
+    20200404:增加可以直接转90度,掉头的功能 by corvin.
  **************************************************************/
-#include "ros/ros.h"
+#include <ros/ros.h>
 #include "ros_arduino_msgs/InfraredSensors.h"
-#include "geometry_msgs/Twist.h"
- 
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_listener.h>
+#include <nav_msgs/Odometry.h>
+
+
 #define  setbit(x, y)  x|=(1<<y)
 #define  clrbit(x, y)  x&=~(1<<y)
 
@@ -24,11 +26,15 @@
 #define  STATUS_F   0x03  // x v v
 #define  STATUS_G   0x05  // v x v
 
+#define  TURN_LEFT   0
+#define  TURN_RIGHT  1
+#define  TURN_BACK   2
+
 //global variable
 geometry_msgs::Twist twist_cmd;
 ros::Publisher twist_pub;
 
-const double warn_dist = 0.25;  //warn check distance
+const double warn_dist = 0.15;  //warn check distance
 
 double default_period_hz = 10;  //hz
 double default_linear_x = 0.15;  // (m/s)
@@ -38,15 +44,12 @@ double front_dist;
 double left_dist;
 double right_dist;
 
-void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
-{
-    ROS_INFO("front distance:[%f]", msg->front_dist);
-    front_dist = msg->front_dist;
-    ROS_INFO("left distance:[%f]", msg->left_dist);
-    left_dist = msg->left_dist;
-    ROS_INFO("right distance:[%f]", msg->right_dist);
-    right_dist = msg->right_dist;
-}
+std::string odomFrame;
+std::string baseFrame;
+
+float linear_speed;
+float angular_speed;
+
 
 void publishTwistCmd(double linear_x, double angular_z)
 {
@@ -61,6 +64,61 @@ void publishTwistCmd(double linear_x, double angular_z)
     twist_pub.publish(twist_cmd);
 }
 
+void controlTurn(int flag)
+{
+    tf::TransformListener tf_Listener;
+    tf::StampedTransform tf_Transform;
+    int angular = 0;
+    int check_angle = 0;
+
+    switch(flag)
+    {
+        case TURN_LEFT:
+            angular = angular_speed;
+            check_angle = M_PI/2;
+            break;
+
+        case TURN_RIGHT:
+            angular = -angular_speed;
+            check_angle = M_PI/2;
+            break;
+
+        case TURN_BACK:
+            check_angle = M_PI;
+            angular = angular_speed;
+            break;
+
+        default:
+            break;
+    }
+
+    try
+    {
+        tf_Listener.waitForTransform(odomFrame, baseFrame, ros::Time(), ros::Duration(2.0));
+    }
+    catch(tf::TransformException &ex)
+    {
+        ROS_ERROR("waitForTransform timeout error !");
+        ros::shutdown();
+    }
+
+    double last_angle = fabs(tf::getYaw(tf_Transform.getRotation()));
+    double turn_angle = 0;
+    while((fabs(turn_angle) < check_angle)&&(ros::ok()))
+    {
+        ROS_INFO("Turning ...");
+        publishTwistCmd(0, angular);
+        ros::Duration(0.05).sleep(); //50 ms
+
+        tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
+        double rotation = fabs(tf::getYaw(tf_Transform.getRotation()));
+        double delta_angle = fabs(rotation - last_angle);
+
+        turn_angle += delta_angle;
+        last_angle = rotation;
+    }
+}
+
 void checkInfraredDist(double infrared_f, double infrared_l, double infrared_r)
 {
    unsigned char flag = 0;
@@ -97,7 +155,8 @@ void checkInfraredDist(double infrared_f, double infrared_l, double infrared_r)
    {
     case STATUS_A: //turn right 0x04
         ROS_WARN("left warn,turn right");
-        publishTwistCmd(0, -default_yaw_rate);
+        //publishTwistCmd(0, -default_yaw_rate);
+        controlTurn(TURN_RIGHT);
         break;
 
     case STATUS_B: // 0x02
@@ -105,65 +164,85 @@ void checkInfraredDist(double infrared_f, double infrared_l, double infrared_r)
         if(infrared_l > infrared_r)
         {
             ROS_WARN("turn left");
-            publishTwistCmd(0, 2*default_yaw_rate);
+            controlTurn(TURN_LEFT);
+            //publishTwistCmd(0, 2*default_yaw_rate);
         }
         else
         {
             ROS_WARN("turn right");
-            publishTwistCmd(0, -default_yaw_rate*2);
+            controlTurn(TURN_RIGHT);
+            //publishTwistCmd(0, -default_yaw_rate*2);
         }
         break;
 
     case STATUS_C: //turn left
         ROS_WARN("left ok, front ok, right warn, turn left");
-        publishTwistCmd(0, default_yaw_rate);
+        //publishTwistCmd(0, default_yaw_rate);
+        controlTurn(TURN_LEFT);
         break;
 
     case STATUS_D:
         ROS_WARN("left,front,right all warn, turn back");
-        publishTwistCmd(0, 10*default_yaw_rate);
+        controlTurn(TURN_BACK);
+        //publishTwistCmd(0, 10*default_yaw_rate);
         break;
 
     case STATUS_E:
         ROS_WARN("left warn, front warn, right ok, turn right");
-        publishTwistCmd(0, (-default_yaw_rate*2));
+        controlTurn(TURN_RIGHT);
+        //publishTwistCmd(0, (-default_yaw_rate*2));
         break;
 
     case STATUS_F:
         ROS_WARN("left ok, front warn, right warn, turn left");
-        publishTwistCmd(0, (default_yaw_rate*2));
+        controlTurn(TURN_LEFT);
+        //publishTwistCmd(0, (default_yaw_rate*2));
         break;
 
     case STATUS_G:
         ROS_WARN("left and right warn, front ok, speed up");
-        publishTwistCmd(2*default_linear_x, 0);
+        publishTwistCmd(default_linear_x, 0);
+        //publishTwistCmd(2*default_linear_x, 0);
         break;
 
     default: //go forward straight line
         publishTwistCmd(default_linear_x, 0);
         break;
    }
+}
 
+void infrared_callback(const ros_arduino_msgs::InfraredSensors::ConstPtr& msg)
+{
+    ROS_INFO("front distance:[%f]", msg->front_dist);
+    front_dist = msg->front_dist;
+
+    ROS_INFO("left distance:[%f]", msg->left_dist);
+    left_dist = msg->left_dist;
+
+    ROS_INFO("right distance:[%f]", msg->right_dist);
+    right_dist = msg->right_dist;
+
+    checkInfraredDist(front_dist, left_dist, right_dist);
 }
 
- 
 int main(int argc, char **argv)
 {
+    std::string infrared_topic;
+    std::string cmd_topic;
+
     ros::init(argc, argv, "infrared_maze_node");
     ros::NodeHandle handle;
-    ros::Rate loop_rate = default_period_hz;
- 
-    ros::Subscriber sub_infrared = handle.subscribe("/mobilebase_arduino/sensor/GP2Y0A41", 100, infrared_callback);
 
-    twist_pub = handle.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
- 
-    while(ros::ok())
-    {
-       checkInfraredDist(front_dist, left_dist, right_dist);
+    ros::param::get("~infrared_topic", infrared_topic);
+    ros::param::get("~cmd_topic",  cmd_topic);
+    ros::param::get("~linear_speed", linear_speed);
+    ros::param::get("~angular_speed", angular_speed);
+    ros::param::get("~odom_frame", odomFrame);
+    ros::param::get("~base_frame", baseFrame);
 
-       ros::spinOnce();
-       loop_rate.sleep();
-    }
- 
+    ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 20, infrared_callback);
+    twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 10);
+
+    ros::spin();
     return 0;
 }

src/robot_bringup/launch/odom_ekf.py

@@ -2,7 +2,7 @@
 
 """ odom_ekf.py - Version 1.1 2013-12-20
 
-    Republish the /robot_pose_ekf/odom_combined topic which is of type 
+    Republish the /robot_pose_ekf/odom_combined topic which is of type
     geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
     type nav_msgs/Odometry so we can view it in RViz.
 
@@ -13,51 +13,44 @@
     it under the terms of the GNU General Public License as published by
     the Free Software Foundation; either version 2 of the License, or
     (at your option) any later version.5
-    
+
     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details at:
-    
+
     http://www.gnu.org/licenses/gpl.html
-      
 """
-
 import rospy
 from geometry_msgs.msg import PoseWithCovarianceStamped
 from nav_msgs.msg import Odometry
 
 class OdomEKF():
     def __init__(self):
-        # Give the node a name
         rospy.init_node('odom_ekf_node', anonymous=False)
 
         # Publisher of type nav_msgs/Odometry
         self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=20)
-        
+
         # Wait for the /odom_combined topic to become available
         rospy.wait_for_message('input', PoseWithCovarianceStamped)
-        
+
         # Subscribe to the /odom_combined topic
         rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
-        
         rospy.loginfo("Publishing combined odometry on /odom_ekf")
-        
+
     def pub_ekf_odom(self, msg):
         odom = Odometry()
         odom.header = msg.header
         odom.header.frame_id = '/odom_combined'
         odom.child_frame_id = 'base_footprint'
         odom.pose = msg.pose
-        
+
         self.ekf_pub.publish(odom)
-        
+
 if __name__ == '__main__':
     try:
         OdomEKF()
         rospy.spin()
     except:
         pass
-        
-
-