update

CMakeLists.txt

@@ -95,6 +95,11 @@ install(FILES README.md LICENSE.md
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
 
+# Install configuration files
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  FILES_MATCHING PATTERN "*.yaml"
+)
 ################################################################################
 # Test
 ################################################################################

config/box_filter.yaml

@@ -1,15 +1,13 @@
-scan_to_scan_filter_chain:
-  ros__parameters:
-    filter1:
-      name: box_filter
-      type: laser_filters/LaserScanBoxFilter
-      params:
-        box_frame: laser #laser_frame
-        max_x: 0.80 #was 0.16
-        max_y: 0.18 #was 0.17
-        max_z: 0.1
-        min_x: -0.15 #was -0.41
-        min_y: -0.18 # was -0.17
-        min_z: -0.2
-        
-        invert: false # activate to remove all points outside of the box
+scan_filter_chain:
+- name: box_filter
+  type: laser_filters/LaserScanBoxFilter
+  params:
+    box_frame: laser #laser_frame
+    max_x: 0.80 #was 0.16
+    max_y: 0.18 #was 0.17
+    max_z: 0.1
+    min_x: -0.15 #was -0.41
+    min_y: -0.18 # was -0.17
+    min_z: -0.2
+    
+    invert: false # activate to remove all points outside of the box

config/twist_mux.yaml

@@ -1,19 +0,0 @@
-twist_mux:
-  ros__parameters:
-    topics:
-      # common:
-      #   topic   : cmd_vel_common
-      #   timeout : 0.5
-      #   priority: 0
-      navigation:
-        topic   : cmd_vel
-        timeout : 0.5
-        priority: 10
-      # tracker:
-      #   topic   : cmd_vel_tracker
-      #   timeout : 0.5
-      #   priority: 20
-      joystick:
-        topic   : cmd_vel_joy
-        timeout : 0.5
-        priority: 50

config/twist_mux_topics.yaml

@@ -0,0 +1,28 @@
+# Input topics handled/muxed.
+# For each topic:
+# - name    : name identifier to select the topic
+# - topic   : input topic of geometry_msgs::Twist type
+# - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead
+# - priority: priority in the range [0, 255]; the higher the more priority over other topics
+
+topics:
+-
+  name    : navigation
+  topic   : nav_vel
+  timeout : 0.5
+  priority: 100
+-
+  name    : joystick
+  topic   : joy_vel
+  timeout : 0.5
+  priority: 90
+-
+  name    : keyboard
+  topic   : key_vel
+  timeout : 0.5
+  priority: 90
+-
+  name    : tablet
+  topic   : tab_vel
+  timeout : 0.5
+  priority: 90

launch/move_base.launch

@@ -4,8 +4,8 @@
   <!--<include file="$(find segwayrmp)/launch/segwayrmp_configuration.launch" />-->
 
   <!-- Run the map server -->
-  <!--<node name="map_server" pkg="map_server" type="map_server" args="$(find segwayrmp)/maps/mymap.pgm my_map_resolution"/>-->
-  <node name="map_server" pkg="map_server" type="map_server" args="$(find segwayrmp)/maps/mymap.yaml"/>
+  <!-- <node name="map_server" pkg="map_server" type="map_server" args="$(find segwayrmp)/maps/mymap.pgm my_map_resolution"/> -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find cps_rmp220_support)/maps/map.yaml"/>
 
   <!--move base-->
   <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
@@ -14,9 +14,15 @@
     <rosparam file="$(find segwayrmp)/param/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find segwayrmp)/param/global_costmap_params.yaml" command="load" />
     <rosparam file="$(find segwayrmp)/param/base_local_planner_params.yaml" command="load" />
+    <!-- scan topic -->
+    <remap from="scan" to="scan_filtered"/>
+    <!-- remap cmd_vel -->
+    <remap from="cmd_vel" to="nav_vel"/>
+    <!-- remap odom -->
+    <remap from="odom" to="/robot_pose_ekf/odom_combined"/>
   </node>
 
-   <!--启动 AMCL 节点 -->
+  <!-- AMCL -->
   <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
     <param name="use_map_topic" value="false"/>
     <!-- Publish scans from best pose at a max of 10 Hz -->
@@ -54,12 +60,16 @@
     <param name="recovery_alpha_slow" value="0.0"/>
     <param name="recovery_alpha_fast" value="0.0"/>
     <!-- scan topic -->
-    <remap from="scan" to="scan"/>
+    <remap from="scan" to="scan_filtered"/>
+    <!-- remap cmd_vel -->
+    <remap from="cmd_vel" to="nav_vel"/>
+    <!-- remap odom -->
+    <remap from="odom" to="/robot_pose_ekf/odom_combined"/>
   </node>
 
   <!-- TF -->
   <!-- <node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.10 0.0 0.12 3.14 0.0 0.0 base_link laser 100"/>
-  <!-- static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms -->  
-  <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0.5 0 0 0 base_link base_footprint 50" /> -->
+  static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms -->  
+  <!-- <node pkg="tf" type="static_transform_publisher" name="base_footprint_to_base_link" args="0 0 0.5 0 0 0 base_link base_footprint 50" /> -->
 
 </launch>

launch/robot_scan_filter.launch

@@ -1,8 +1,7 @@
 <launch>
-    <!-- Include the configuration file -->
-    <param name="box_filter_config" command="$(find cps_rmp220_support)/config/box_filter.yaml" />
-
-    <!-- Launch the laser_filters node -->
-    <node pkg="laser_filters" type="scan_to_scan_filter_chain" name="scan_to_scan_filter_chain" output="screen">/>
-    </node>
-</launch>
+  <node pkg="laser_filters" type="scan_to_scan_filter_chain"
+      name="laser_filter">
+    <rosparam command="load" file="$(find cps_rmp220_support)/config/box_filter.yaml" />
+    <!-- <remap from="scan" to="base_scan" /> -->
+  </node>
+</launch>

launch/twist_mux.launch

@@ -0,0 +1,30 @@
+<launch>
+  <arg name="joy_vel_in"  default="input_joy/cmd_vel"/>
+  <arg name="joy_vel_out" default="joy_vel"/>
+
+  <arg name="cmd_vel_out" default="twist_mux/cmd_vel"/>
+
+  <arg name="config_locks"  default="$(find twist_mux)/config/twist_mux_locks.yaml"/>
+  <arg name="config_topics" default="$(find cps_rmp220_Support)/config/twist_mux_topics.yaml"/>
+
+  <arg name="config_joy" default="$(find twist_mux)/config/joystick.yaml"/>
+
+  <node pkg="twist_mux" type="twist_mux" name="twist_mux" output="screen">
+    <remap from="cmd_vel_out" to="$(arg cmd_vel_out)"/>
+
+    <rosparam file="$(arg config_locks)"  command="load"/>
+    <rosparam file="$(arg config_topics)" command="load"/>
+  </node>
+
+  <node pkg="twist_mux" type="twist_marker" name="twist_marker">
+    <remap from="twist"  to="$(arg cmd_vel_out)"/>
+    <remap from="marker" to="twist_marker"/>
+  </node>
+
+  <node pkg="twist_mux" type="joystick_relay.py" name="joystick_relay" output="screen">
+    <remap from="joy_vel_in"  to="$(arg joy_vel_in)"/>
+    <remap from="joy_vel_out" to="$(arg joy_vel_out)"/>
+
+    <rosparam file="$(arg config_joy)" command="load"/>
+  </node>
+</launch>

maps/map.yaml

@@ -0,0 +1,6 @@
+image: map.pgm
+resolution: 0.050000
+origin:  [-100.000000, -90.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

nodes/encoder_odom_publisher.py

@@ -26,7 +26,7 @@ class EncoderOdometry:
 
         # ROS publishers (Updated topic to /odom_new)
         self.odom_pub = rospy.Publisher("/odom_new", Odometry, queue_size=10)
-        self.odom_broadcaster = tf.TransformBroadcaster()
+        #self.odom_broadcaster = tf.TransformBroadcaster()
 
         # ROS subscriber to /ticks_fb topic
         rospy.Subscriber("/ticks_fb", ticks_fb, self.ticks_callback)