增加view_navigation.rviz的修改

src/robot_bringup/launch/odom_ekf.launch

@@ -1,6 +0,0 @@
-<launch>
-  <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
-    <remap from="input" to="/robot_pose_ekf/odom_combined"/>
-    <remap from="output" to="/odom_ekf"/>
-  </node>
-</launch>

src/robot_bringup/launch/robot_bringup.launch

@@ -12,13 +12,13 @@
 
     <!-- (2) startup mobilebase arduino launch -->
     <include file="$(find ros_arduino_python)/launch/arduino.launch" />
-    
+
     <!-- (3) startup rasp imu-6DOF board-->
     <include file="$(find rasp_imu_hat_6dof)/launch/imu_data_pub.launch" />
 
     <!-- (4) startup robot_pose_ekf node -->
     <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
-        <param name="output_frame" value="odom"/>
+        <param name="output_frame" value="odom_combined"/>
         <param name="freq" value="10.0"/>
         <param name="sensor_timeout" value="1.0"/>
         <param name="odom_used" value="true"/>
@@ -27,5 +27,11 @@
         <param name="debug" value="false"/>
         <param name="self_diagnose" value="false"/>
   </node>
+
+  <!-- (5) /odom_combined view in rviz>
+  <node pkg="robot_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
+      <remap from="input" to="/robot_pose_ekf/odom_combined" />
+      <remap from="output" to="/odom_ekf" />
+  </node>
 </launch>
 

src/robot_description/urdf/robot.urdf

@@ -1,5 +1,4 @@
 <robot name="robot">
-
   <material name="blue">
     <color rgba="0.1 0.1 0.75 0.9"/>
   </material>
@@ -54,6 +53,19 @@
     <child link="base_link"/>
   </joint>
 
+  <link name="base_imu_link" type="fixed">
+    <visual>
+      <geometry>
+        <box size="0.056 0.065 0.002" />
+      </geometry>
+        <material name="blue" />
+    </visual>
+  <joint name="base_link_to_IMU" type="fixed">
+    <origin xyz="0 0 0.10" />
+    <parent link="base_link" />
+    <child link="base_imu_link" />
+  </joint>
+
   <link name="lidar" type="fixed">
     <inertial>
       <origin>

src/robot_navigation/config/base_local_planner_params.yaml

@@ -20,13 +20,13 @@ clearing_rotation_allowed: false
 
 TrajectoryPlannerROS:
    #Robot Configuration Parameters
-   max_vel_x: 0.17
-   min_vel_x: 0.12
+   max_vel_x: 0.15
+   min_vel_x: 0.10
    y_vels: [-0.17,-0.1,0.1,0.17]
    max_vel_theta: 0.5
    min_vel_theta: -0.5
    min_in_place_vel_theta: 0.3
-   escape_vel: -0.12
+   escape_vel: -0.10
 
    acc_lim_x: 0.05
    acc_lim_y: 0.05   # accelerator speed 
@@ -66,4 +66,4 @@ TrajectoryPlannerROS:
 
    #Global Plan Parameters
    prune_plan: true
-   
+

src/robot_navigation/config/costmap_common_params.yaml

@@ -4,7 +4,7 @@
 # 代价地图通用配置文件，各参数意义如下：
 #   obstacle_range: 最大障碍物检测范围,超过该范围不认为是障碍物
 #   raytrace_range: 检测自由空间的最大范围
-#   robot_radius: 机器人本体的半径
+#   robot_radius: 机器人本体的半径大小,单位:米
 #   inflation_radius: 与障碍物的安全半径距离,如果机器人经常撞到
 #     障碍物就需要增大该值,若无法通过狭窄地方就减小该值
 #   observation_sources: 观察源是激光雷达
@@ -15,6 +15,7 @@ obstacle_range: 2.0
 raytrace_range: 3.0
 robot_radius: 0.126
 inflation_radius: 0.2
+
 observation_sources: scan
 scan: {data_type: LaserScan, topic: scan, marking: true, clearing: true}
 

src/robot_navigation/config/global_costmap_params.yaml

@@ -16,7 +16,7 @@
 global_costmap:
    global_frame: /map
    robot_base_frame: /base_footprint
-   update_frequency: 2.0
+   update_frequency: 1.0
    publish_frequency: 0
    static_map: true
    rolling_window: false

src/robot_navigation/config/local_costmap_params.yaml

@@ -17,7 +17,7 @@
 #
 
 local_costmap:
-   global_frame: /odom
+   global_frame: /odom_combined
    robot_base_frame: /base_footprint
    update_frequency: 3.0
    publish_frequency: 2.0

src/robot_navigation/launch/amcl_rplidar_a1.launch

@@ -0,0 +1,53 @@
+<launch>
+  <!-- Run the map server with a map -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find robot_navigation)/maps/my_room.yaml" />
+
+  <!-- startup rplidar node -->
+  <include file="$(find robot_navigation)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find robot_navigation)/launch/move_base.launch" />
+
+  <!-- Run amcl -->
+  <node pkg="amcl" type="amcl" name="robot_amcl">
+    <param name="odom_model_type" value="omni"/>
+
+    <param name="base_frame_id" value="base_footprint"/>
+    <param name="global_frame_id" value="map"/>
+    <param name="odom_frame_id" value="odom_combined"/>
+
+    <param name="use_map_topic" value="true"/>
+    <param name="odom_alpha5" value="0.1"/>
+    <param name="transform_tolerance" value="0.5" />
+    <param name="gui_publish_rate" value="1.0"/>
+    <param name="laser_max_beams" value="300"/>
+    <param name="min_particles" value="500"/>
+    <param name="max_particles" value="5000"/>
+    <param name="kld_err" value="0.1"/>
+    <param name="kld_z" value="0.99"/>
+    <param name="odom_alpha1" value="0.2"/>
+    <param name="odom_alpha2" value="0.2"/>
+    <param name="odom_alpha3" value="0.2"/>
+    <param name="odom_alpha4" value="0.2"/>
+    <param name="odom_alpha5" value="0.2"/>
+
+    <param name="laser_z_hit" value="0.9"/>
+    <param name="laser_z_short" value="0.05"/>
+    <param name="laser_z_max" value="0.05"/>
+    <param name="laser_z_rand" value="0.05"/>
+    <param name="laser_sigma_hit" value="0.2"/>
+    <param name="laser_lambda_short" value="0.1"/>
+    <param name="laser_model_type" value="likelihood_field"/>
+    <param name="laser_model_type" value="beam"/>
+    <param name="laser_min_range" value="0.14"/>
+    <param name="laser_max_range" value="7.0"/>
+    <param name="laser_likelihood_max_dist" value="2.0"/>
+    <param name="update_min_d" value="0.2"/>
+    <param name="update_min_a" value="0.5"/>
+    <param name="resample_interval" value="2"/>
+    <param name="transform_tolerance" value="3.0"/>
+    <param name="recovery_alpha_slow" value="0.0"/>
+    <param name="recovery_alpha_fast" value="0.0"/>
+  </node>
+</launch>
+

src/robot_navigation/launch/gmapping_rplidar_a1.launch

@@ -0,0 +1,43 @@
+<launch>
+  <param name="use_sim_time" value="false"/>  
+
+  <!-- startup rplidar node -->
+  <include file="$(find robot_navigation)/launch/rplidar.launch" />
+
+  <!-- startup move_base node -->
+  <include file="$(find robot_navigation)/launch/move_base.launch" />
+
+  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">  
+    <param name="odom_frame" value="odom_combined" />
+
+    <param name="map_update_interval" value="5.0"/>
+    <param name="maxUrange" value="7.5"/>
+    <param name="sigma" value="0.05"/>
+    <param name="kernelSize" value="1"/>
+    <param name="lstep" value="0.05"/>
+    <param name="astep" value="0.05"/>
+    <param name="iterations" value="5"/>
+    <param name="lsigma" value="0.075"/>
+    <param name="ogain" value="3.0"/>
+    <param name="lskip" value="0"/>
+    <param name="minimumScore" value="50"/>
+    <param name="srr" value="0.1"/>
+    <param name="srt" value="0.2"/>
+    <param name="str" value="0.1"/>
+    <param name="stt" value="0.2"/>
+    <param name="linearUpdate" value="1.0"/>
+    <param name="angularUpdate" value="0.5"/>
+    <param name="temporalUpdate" value="3.0"/>
+    <param name="resampleThreshold" value="0.5"/>
+    <param name="particles" value="30"/>
+    <param name="xmin" value="-5.0"/>
+    <param name="ymin" value="-5.0"/>
+    <param name="xmax" value="5.0"/>
+    <param name="ymax" value="5.0"/>
+    <param name="delta" value="0.05"/>
+    <param name="llsamplerange" value="0.01"/>
+    <param name="llsamplestep" value="0.01"/>
+    <param name="lasamplerange" value="0.005"/>
+    <param name="lasamplestep" value="0.005"/>
+  </node>
+</launch>

src/robot_navigation/launch/move_base.launch

@@ -12,11 +12,15 @@
 -->
 <launch>
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
+    <param name="controller_frequency" value="9" />
+    <param name="controller_patiente" value="10" />
+
     <rosparam file="$(find robot_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
     <rosparam file="$(find robot_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
     <rosparam file="$(find robot_navigation)/config/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find robot_navigation)/config/global_costmap_params.yaml" command="load" />
     <rosparam file="$(find robot_navigation)/config/base_local_planner_params.yaml" command="load" />
+
     <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
   </node>
 </launch>

src/robot_navigation/rviz/view_navigation.rviz

@@ -8,10 +8,14 @@ Panels:
         - /TF1/Tree1
         - /Global Map1/Planner1
         - /Local Map1/Planner1
+<<<<<<< HEAD
         - /Odometry1
         - /Odometry1/Shape1
         - /Imu1
         - /Imu1/Axes properties1
+=======
+        - /Odometry1/Shape1
+>>>>>>> 1dfea8ba86879c7d7ea5698413e1aa671262be0d
       Splitter Ratio: 0.605556011
     Tree Height: 663
   - Class: rviz/Selection
@@ -53,7 +57,7 @@ Visualization Manager:
         Y: 0
         Z: 0
       Plane: XY
-      Plane Cell Count: 10
+      Plane Cell Count: 30
       Reference Frame: <Fixed Frame>
       Value: true
     - Alpha: 1
@@ -139,9 +143,9 @@ Visualization Manager:
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 253
+      Max Intensity: 47
       Min Color: 0; 0; 0
-      Min Intensity: 252
+      Min Intensity: 47
       Name: LaserScan
       Position Transformer: XYZ
       Queue Size: 1000
@@ -389,41 +393,25 @@ Visualization Manager:
       Name: Odometry
       Position Tolerance: 0.200000003
       Shape:
-        Alpha: 1
+        Alpha: 0.699999988
         Axes Length: 1
         Axes Radius: 0.100000001
+<<<<<<< HEAD
         Color: 57; 95; 199
         Head Length: 0.100000001
         Head Radius: 0.100000001
         Shaft Length: 0.100000001
+=======
+        Color: 31; 87; 239
+        Head Length: 0.100000001
+        Head Radius: 0.0500000007
+        Shaft Length: 0.150000006
+>>>>>>> 1dfea8ba86879c7d7ea5698413e1aa671262be0d
         Shaft Radius: 0.0500000007
         Value: Arrow
       Topic: /odom_ekf
       Unreliable: false
       Value: true
-    - Acceleration properties:
-        Acc. vector alpha: 1
-        Acc. vector color: 255; 0; 0
-        Acc. vector scale: 1
-        Derotate acceleration: true
-        Enable acceleration: false
-      Axes properties:
-        Axes scale: 1
-        Enable axes: true
-      Box properties:
-        Box alpha: 1
-        Box color: 79; 162; 86
-        Enable box: true
-        x_scale: 0.0500000007
-        y_scale: 0.0500000007
-        z_scale: 0.0500000007
-      Class: rviz_imu_plugin/Imu
-      Enabled: true
-      Name: Imu
-      Topic: /imu
-      Unreliable: false
-      Value: true
-      fixed_frame_orientation: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -448,25 +436,42 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
+<<<<<<< HEAD
       Distance: 6.24388552
+=======
+      Distance: 7.83131266
+>>>>>>> 1dfea8ba86879c7d7ea5698413e1aa671262be0d
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
+<<<<<<< HEAD
         X: -0.350812674
         Y: 1.04504514
         Z: -1.69744515
+=======
+        X: 2.21107078
+        Y: 1.08232093
+        Z: 0.0960710421
+>>>>>>> 1dfea8ba86879c7d7ea5698413e1aa671262be0d
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.0500000007
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.00999999978
+<<<<<<< HEAD
       Pitch: 1.31979465
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
       Yaw: 0.0949685201
+=======
+      Pitch: 0.859797716
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 4.52996302
+>>>>>>> 1dfea8ba86879c7d7ea5698413e1aa671262be0d
     Saved: ~
 Window Geometry:
   Displays:

src/ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml

@@ -5,6 +5,8 @@ is_use_serial: True
 
 serial_port: /dev/ttyACM0
 serial_baud: 57600
+
+i2c_smbus_num: 1   #raspberryPi:1, huawei altals 200DK: 2
 i2c_slave_addr: 0x08  #Get arduino due i2c slave addr
 
 timeout: 0.5
@@ -23,11 +25,11 @@ odom_name: odom
 
 # === Robot drivetrain parameters
 wheel_diameter: 0.058
-wheel_track: 0.126     #L value
+wheel_track: 0.114     #L value
 encoder_resolution: 11 #12V DC motors
 gear_reduction: 46     #empty payload rpm 130r/m
 debugPID: False
-linear_scale_correction: 1.00 
+linear_scale_correction: 0.9886
 angular_scale_correction: 1.00
 
 # === PID parameters

src/ros_arduino_bridge/ros_arduino_python/nodes/arduino_node.py

@@ -28,6 +28,7 @@ class ArduinoROS():
 
         self.serial_port = rospy.get_param("~serial_port", "/dev/ttyACM0")
         self.serial_baud = int(rospy.get_param("~serial_baud", 57600))
+        self.i2c_smbus_num = rospy.get_param("~i2c_smbus_num", 1)
         self.i2c_slave_addr = rospy.get_param("~i2c_slave_addr", 8)
         self.timeout    = rospy.get_param("~timeout", 0.7)
         self.base_frame = rospy.get_param("~base_frame", 'base_footprint')
@@ -76,7 +77,7 @@ class ArduinoROS():
         rospy.Service('~light_show', LightShow, self.LightShowHandler)
 
         # Initialize the controlller
-        self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_slave_addr, self.timeout)
+        self.controller = Arduino(self.is_use_serial, self.serial_port, self.serial_baud, self.i2c_smbus_num, self.i2c_slave_addr, self.timeout)
 
         # Make the connection
         self.controller.connect()

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

@@ -22,7 +22,7 @@ class Arduino:
     N_ANALOG_PORTS  = 10
     N_DIGITAL_PORTS = 54
 
-    def __init__(self, is_use_serial, serial_port="/dev/ttyACM0", baudrate=57600, i2c_slave_addr=8, timeout=0.5):
+    def __init__(self, is_use_serial, serial_port="/dev/ttyACM0", baudrate=57600,i2c_smbus_num=1, i2c_slave_addr=8, timeout=0.5):
         self.PID_RATE = 40 # Do not change this!  It is a fixed property of the Arduino PID controller.
         self.PID_INTERVAL = 25
 
@@ -30,6 +30,7 @@ class Arduino:
         self.serial_port   = serial_port
         self.baudrate = baudrate
 
+        self.i2c_smbus_num = i2c_smbus_num
         self.i2c_slave_addr = i2c_slave_addr
 
         self.timeout  = timeout
@@ -51,7 +52,7 @@ class Arduino:
         if self.is_use_serial:
             self.serial_connect()
         else:
-            self.i2c_bus = smbus.SMBus(1)
+            self.i2c_bus = smbus.SMBus(self.i2c_smbus_num)
             self.i2c_connect()
 
     def serial_connect(self):
@@ -314,14 +315,30 @@ class Arduino:
             return values
 
     def i2c_get_encoder_counts(self):
-        print "IIC Get Encoder count !"
-        #values = self.execute_array('e')
-        #if len(values) != 3:
-        #    print "Encoder count was not 3"
-        #    raise SerialException
-        #    return None
-        #else:
-        #    return values
+        #print "IIC Get Encoder count !"
+        ch = ''
+        values = ''
+        cnt = 0
+        cmd = "\r"
+
+        try:
+            self.i2c_bus.write_byte(self.i2c_addr, int(ord('e')))
+            self.i2c_bus.write_byte(self.i2c_addr, ord('\r'))
+
+            result_string = ''.join([chr(e) for e in self.i2c_bus.read_i2c_block_data(self.i2c_addr, 0x06)])
+            result_flag = result_string.index('\r')
+            values=[int(e) for e in ''.join(result_string[:result_flag]).split('')]
+        except:
+            print sys.exe_info()
+            traceback.print_exc(file=sys.stdout)
+            return None
+
+        if len(values) != 3:
+            print "Encoder count was not 3"
+            return None
+        else:
+            return values
+
 
     def reset_encoders(self):
         ''' Reset the encoder counts to 0 by serial port

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -219,6 +219,7 @@ class BaseController:
 
             # create the odometry transform frame broadcaster.
             # when startup robot_pose_ekf, should disable this broadcaster
+            #
             #self.odomBroadcaster.sendTransform(
             #    (self.x, self.y, 0),
             #    (quaternion.x, quaternion.y, quaternion.z, quaternion.w),