新增功能可以通过服务调用方式获取红外测距传感器距离值,在走迷宫时，直行时检测前方距离,到达警告距离则停止前进

src/infrared_maze/cfg/param.yaml

@@ -6,10 +6,12 @@
 # History:
 #   20200404:init this file.
 #   20200406:增加yaw service,rate参数.
+#   20200412:增加获取红外测距信息的service.
 ######################################################
 infrared_topic: /mobilebase_arduino/sensor/GP2Y0A41
 cmd_topic: /cmd_vel
 yaw_service: /imu_node/GetYawData
+dist_service: /mobilebase_arduino/getInfraredDistance
 linear_x: 0.17
 linear_y: 0.15
 angular_speed: 0.5

src/infrared_maze/src/infrared_maze.cpp

@@ -4,6 +4,7 @@
  Author: jally, corvin
  History:
     20200404:增加可以直接转90度,掉头的功能 by corvin.
+    20200412:增加使用服务方式来获取红外测距值-corvin.
  **************************************************************/
 #include <ros/ros.h>
 #include <std_msgs/Float32.h>
@@ -12,6 +13,7 @@
 #include <tf/transform_listener.h>
 #include "rasp_imu_hat_6dof/GetYawData.h"
 #include "ros_arduino_msgs/InfraredSensors.h"
+#include "ros_arduino_msgs/GetInfraredDistance.h"
 
 
 #define  GO_FORWARD   0
@@ -22,11 +24,15 @@
 
 //global variable
 ros::Publisher twist_pub;
-ros::ServiceClient srvClient;
-rasp_imu_hat_6dof::GetYawData srv;
+ros::ServiceClient yawSrvClient;
+rasp_imu_hat_6dof::GetYawData yawSrv;
+
+//红外测距相关的服务
+ros::ServiceClient distSrvClient;
+ros_arduino_msgs::GetInfraredDistance distSrv;
 
 float warn_dist = 0.15;  //warn check distance
-float tolerance_dist = 0.04; //对警告距离的容忍值
+float tolerance_dist = 0.05; //对警告距离的容忍值
 
 float front_dist;
 float left_dist;
@@ -65,10 +71,10 @@ void publishTwistCmd(float linear_x, float linear_y, float angular_z)
  * Description:控制小车移动在过道中间,比较左右两边的距离后
  *   然后进行水平左右移动.
  **********************************************************/
-void horizMoveMiddle(float left,float right,float tolerance)
+void horizMoveMiddle(float left, float right, float tolerance)
 {
     float diff = left - right;
-    ROS_INFO("diff:%f,left:%f,right:%f",diff,left,right);
+    ROS_INFO("diff:%f,left:%f,right:%f",diff, left, right);
     if((fabs(diff) > tolerance)&&(left!=(float)0.3)&&(right!=(float)0.3))
     {
         if(diff < 0) //move right
@@ -98,6 +104,7 @@ void goForward()
     tf::StampedTransform tf_Transform;
     float dist = 0.0;
     float check_dist = 0.50;
+    float front_dist = 0.0;
 
     try
     {
@@ -106,14 +113,18 @@ void goForward()
     catch(tf::TransformException &ex)
     {
         ROS_ERROR("tf_Listener.waitForTransform timeout error !");
-        ROS_ERROR("%s",ex.what());
+        ROS_ERROR("%s", ex.what());
         ros::shutdown();
     }
 
     tf_Listener.lookupTransform(odomFrame, baseFrame, ros::Time(0), tf_Transform);
     float x_start = tf_Transform.getOrigin().x();
     float y_start = tf_Transform.getOrigin().y();
-    while((dist < check_dist) && (ros::ok()))
+
+    distSrvClient.call(distSrv); //通过服务调用来获取测距值
+    front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
+    ROS_WARN("Front dist:%f", front_dist);
+    while((dist<check_dist)&&(front_dist>tolerance_dist*3.0)&&(ros::ok()))
     {
         ROS_INFO("Go forward, dist:%f", dist);
         publishTwistCmd(linear_x_speed, 0, 0);
@@ -123,6 +134,10 @@ void goForward()
         float x = tf_Transform.getOrigin().x();
         float y = tf_Transform.getOrigin().y();
         dist = sqrt(pow((x - x_start), 2) + pow((y - y_start), 2));
+
+        distSrvClient.call(distSrv); //通过服务调用来获取测距值
+        front_dist = distSrv.response.front_dist; //记录当前测距值,防止撞墙
+        ROS_WARN("Front dist:%f", front_dist);
     }
     publishTwistCmd(0, 0, 0); //stop move
     ROS_INFO("Go forward finish !!!");
@@ -186,7 +201,7 @@ 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()))
     {
         publishTwistCmd(0, 0, angular); //原地转弯,不需要linear_x,y的控制
@@ -226,8 +241,8 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
 {
     static int flag = 0;
 
-    srvClient.call(srv); //通过服务调用来获取yaw值
-    start_yaw_data = srv.response.yaw; //记录当前yaw值,用于后面修正
+    yawSrvClient.call(yawSrv); //通过服务调用来获取yaw值
+    start_yaw_data = yawSrv.response.yaw; //记录当前yaw值,用于后面修正
 
     //判断能否左转,并且上一次动作不能是转弯
     if((infrared_l == (float)0.3)&&(flag != 1))
@@ -267,7 +282,7 @@ void checkInfraredDist(float infrared_f,float infrared_l,float infrared_r,
  *                0  -0
  *                -----
  *              /      \
- *         1.57 |      | -1.57
+ *         1.57|        | -1.57
  *              \      /
  *               ------
  *             3.14 -3.14
@@ -312,8 +327,8 @@ void yawUpdate(const float start_yaw)
     //开始不断的计算偏差,调整角度
     while(fabs(diff) > 0.01)
     {
-        srvClient.call(srv);
-        now_yaw = srv.response.yaw;
+        yawSrvClient.call(yawSrv);
+        now_yaw = yawSrv.response.yaw;
         ROS_INFO("Now Yaw data:%f, Target Yaw:%f", now_yaw, target);
 
         diff = target - now_yaw;
@@ -360,6 +375,7 @@ int main(int argc, char **argv)
     std::string infrared_topic;
     std::string cmd_topic;
     std::string yaw_service;
+    std::string dist_service;
 
     ros::init(argc, argv, "infrared_maze_node");
     ros::NodeHandle handle;
@@ -367,6 +383,7 @@ int main(int argc, char **argv)
     ros::param::get("~infrared_topic", infrared_topic);
     ros::param::get("~cmd_topic", cmd_topic);
     ros::param::get("~yaw_service", yaw_service);
+    ros::param::get("~dist_service", dist_service);
     ros::param::get("~linear_x",  linear_x_speed);
     ros::param::get("~linear_y",  linear_y_speed);
     ros::param::get("~angular_speed", angular_speed);
@@ -377,7 +394,8 @@ int main(int argc, char **argv)
 
     ros::Subscriber sub_infrared = handle.subscribe(infrared_topic, 1, infrared_callback);
     twist_pub = handle.advertise<geometry_msgs::Twist>(cmd_topic, 1);
-    srvClient = handle.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
+    yawSrvClient = handle.serviceClient<rasp_imu_hat_6dof::GetYawData>(yaw_service);
+    distSrvClient = handle.serviceClient<ros_arduino_msgs::GetInfraredDistance>(dist_service);
 
     ros::spin();
     return 0;

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -24,11 +24,12 @@ add_service_files(FILES
                   LightShow.srv
                   GripperControl.srv
                   GripperAngle.srv
+                  GetInfraredDistance.srv
                  )
 
-generate_messages(   
-	DEPENDENCIES  
-	std_msgs  
-)  
+generate_messages(
+	DEPENDENCIES
+	std_msgs
+)
 
 catkin_package(CATKIN_DEPENDS message_runtime std_msgs)

src/ros_arduino_bridge/ros_arduino_msgs/package.xml

@@ -4,11 +4,11 @@
   <description>
     ROS Arduino Messages.
   </description>
-  <author>Patrick Goebel</author>
-  <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
+  <author>corvin zhang</author>
+  <maintainer email="corvin_zhang@corvin.cn">corvin zhang</maintainer>
   <license>BSD</license>
   <url>http://ros.org/wiki/ros_arduino_msgs</url>
-  
+
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>message_generation</build_depend>
@@ -16,5 +16,4 @@
 
   <run_depend>message_runtime</run_depend>
   <run_depend>std_msgs</run_depend>
-  
 </package>

src/ros_arduino_bridge/ros_arduino_msgs/srv/GetInfraredDistance.srv

@@ -0,0 +1,4 @@
+---
+float32 front_dist
+float32 left_dist
+float32 right_dist

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -44,7 +44,7 @@ debugPID: False
 
 accel_limit: 0.05
 
-AWheel_Kp: 25
+AWheel_Kp: 22
 AWheel_Kd: 34
 AWheel_Ki: 0
 AWheel_Ko: 50

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -1,7 +1,12 @@
 #!/usr/bin/env python
+#_*_ coding:utf-8 _*_
 
 """
-    A ROS Node for the Arduino microcontroller
+  Copyright:2016-2020 www.corvin.cn ROS小课堂
+  Description: A ROS Node for the Arduino microcontroller
+  Author: corvin
+  History:
+    20200412:init this file.
 """
 import os
 import rospy
@@ -13,6 +18,7 @@ from ros_arduino_python.arduino_sensors import *
 from ros_arduino_python.arduino_driver import Arduino
 from ros_arduino_python.base_controller import BaseController
 
+
 class ArduinoROS():
     def __init__(self):
         rospy.init_node('mobilebase_arduino_node', log_level=rospy.INFO)
@@ -69,6 +75,9 @@ class ArduinoROS():
         # A service to control servo angle
         rospy.Service('~gripper_angle', GripperAngle, self.GripperAngleHandler)
 
+        # A service to return infrared sensor distance
+        rospy.Service('~getInfraredDistance', GetInfraredDistance, self.GetInfraredDistanceHandler)
+
         # Initialize the controlller
         self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
 
@@ -160,6 +169,10 @@ class ArduinoROS():
         self.controller.gripper_angle(req.angle)
         return GripperAngleResponse()
 
+    def GetInfraredDistanceHandler(self, req):
+        value = self.controller.detect_distance()
+        return GetInfraredDistanceResponse(value[0]/100.0,value[1]/100.0,value[2]/100.0)
+
     # Stop the robot
     def shutdown(self):
         rospy.logwarn("Shutting down Arduino Node...")

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -7,6 +7,7 @@
   Author: corvin
   History:
     20200329:增加获取电池剩余电量百分比函数.
+    20200412:增加发布红外测距信息的服务.
 """
 from serial.serialutil import SerialException
 import thread, smbus, rospy, time, os
@@ -406,7 +407,6 @@ class Arduino:
         return self.execute('f')
 
     def detect_distance(self): #检测三个红外测距传感器的返回值,一共返回三个值,分别为前,左,右三个测距值
-        #rospy.loginfo("detect distance")
         return self.execute_array('h')
 
     def getBatPercent(self): #获取电池的剩余电量百分比