CLOBOT-Co-Ltd · morty-clobot · Sep 28, 2021
diff --git a/clober_description/urdf/clober.urdf.xacro b/clober_description/urdf/clober.urdf.xacro
@@ -9,6 +9,8 @@
 
     <xacro:property name="PI" value="3.1415926535897931"/>
 
+    <xacro:arg name="prefix" default="clober_0/"/>
+
     <link name="$(arg prefix)base_footprint"/>
 
     <joint name="$(arg prefix)base_joint" type="fixed">

diff --git a/clober_mbf/CHANGELOG.rst b/clober_mbf/CHANGELOG.rst
@@ -0,0 +1,7 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package clober_mbf
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.5 (2021-09-28)
+------------------
+* Initial clober_mbf package
diff --git a/clober_mbf/CMakeLists.txt b/clober_mbf/CMakeLists.txt
@@ -0,0 +1,11 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(clober_mbf)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+install(
+  DIRECTORY config launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
diff --git a/clober_mbf/README.md b/clober_mbf/README.md
@@ -0,0 +1,24 @@
+# Clober MBF(Move Base Flex)
+**Clober may move & rotate for navigation. Make sure you are operating in a safe environment**
+
+MBF is a highly flexible navigation framework. See this [*link*](http://wiki.ros.org/move_base_flex) to have more information of MBF
+
+clober support mbf based navigation, so that it makes Clober can deal with more complex tasks.
+
+[*SLAM of the environment allow you to acquire a map*](https://github.com/clobot-git/clober/tree/noetic-devel/clober_slam)
+
+## 1. Run MBF Navigation Nodes
+### 1.1 Bringup Robot
+1. Run a `Bringup` for the Clober.
+  ```bash
+  roslaunch clober_bringup base.launch
+  ```
+- This can be substituted by running a simulation node
+  ```bash
+  roslaunch clober_simulation logo_world.launch
+  ```
+
+### 1.2 Launch Navigation
+```bash
+roslaunch clober_mbf navigation.launch
+```
diff --git a/clober_mbf/config/clobot_local_planner_params.yaml b/clober_mbf/config/clobot_local_planner_params.yaml
@@ -0,0 +1,7 @@
+CLOBOTPlannerROS:
+  max_vel_x: 0.7                      #robot max traslational velocity
+  follow_curve_threshlod: 0.2         #if value > follow_curve_threshold, then robot do alignment
+  alignment_threshold: 0.05           #robot target angle alignment threshold [rad]
+  alignment_vel: 0.5
+  goal_threshold: 0.1                 #robot target position distance threshold [m] 
+  following_point: 20                 #The path index for robot following
diff --git a/clober_mbf/config/controllers.yaml b/clober_mbf/config/controllers.yaml
@@ -0,0 +1,85 @@
+controllers:
+# You can add more controllers you need to use.
+#- name: ''
+#  type: ''
+ - name: 'CLOBOTPlannerROS'
+   type: 'clobot_local_planner/CLOBOTPlannerROS'
+ - name: 'tra'
+   type: 'base_local_planner/TrajectoryPlannerROS'
+ - name: 'dwa'
+   type: 'dwa_local_planner/DWAPlannerROS'
+
+# Set configuration of CLOBOTPlannerROS aka. 'CLOBOTPlannerROS'
+CLOBOTPlannerROS:
+  max_vel_x: 0.7                      #robot max traslational velocity
+  follow_curve_threshlod: 0.2         #if value > follow_curve_threshold, then robot do alignment
+  alignment_threshold: 0.05           #robot target angle alignment threshold [rad]
+  alignment_vel: 0.5
+  goal_threshold: 0.1                 #robot target position distance threshold [m] 
+  following_point: 20                 #The path index for robot following
+
+
+# Set configuration of TrajectoryPlannerROS aka. 'tra'
+tra:
+  max_vel_x: 0.8
+  min_vel_x: 0.01
+  max_vel_theta: 0.5
+  min_in_place_vel_theta: 0.3
+
+  acc_lim_theta: 1.0
+  acc_lim_x: 1.0
+  acc_lim_y: 0.0
+
+  holonomic_robot: false
+
+  meter_scoring : true
+
+  controller_frequency: 10.0
+
+# Set configuration of DWAPlannerROS aka. 'dwa'
+dwa:
+# Robot Configuration Parameters
+  max_vel_x: 0.26
+  min_vel_x: -0.26
+
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+
+# The velocity when robot is moving in a straight line
+  max_vel_trans:  0.26
+  min_vel_trans:  0.13
+
+  max_vel_theta: 1.82
+  min_vel_theta: 0.9
+
+  acc_lim_x: 2.5
+  acc_lim_y: 0.0
+  acc_lim_theta: 3.2 
+
+# Goal Tolerance Parametes
+  xy_goal_tolerance: 0.05
+  yaw_goal_tolerance: 0.17
+  latch_xy_goal_tolerance: false
+
+# Forward Simulation Parameters
+  sim_time: 2.0
+  vx_samples: 20
+  vy_samples: 0
+  vth_samples: 40
+  controller_frequency: 10.0
+
+# Trajectory Scoring Parameters
+  path_distance_bias: 32.0
+  goal_distance_bias: 20.0
+  occdist_scale: 0.02
+  forward_point_distance: 0.325
+  stop_time_buffer: 0.2
+  scaling_speed: 0.25
+  max_scaling_factor: 0.2
+
+# Oscillation Prevention Parameters
+  oscillation_reset_dist: 0.05
+
+# Debugging
+  publish_traj_pc : true
+  publish_cost_grid_pc: true
diff --git a/clober_mbf/config/costmap_common.yaml b/clober_mbf/config/costmap_common.yaml
@@ -0,0 +1,8 @@
+footprint: [[-0.202, -0.202], [-0.202, 0.202], [0.202, 0.202], [0.202, -0.202]] 
+
+update_frequency: 2
+publish_frequency: 2
+
+obstacle_layer:
+  observation_sources: scan
+  scan: {topic: scan, sensor_frame: laser_link, data_type: LaserScan, clearing: true, marking: true, obstacle_range: 3.0, raytrace_range: 3.5}
diff --git a/clober_mbf/config/global_costmap.yaml b/clober_mbf/config/global_costmap.yaml
@@ -0,0 +1,16 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+
+  static_map: true
+  rolling_window: false
+
+  plugins:
+    - name: static_map
+      type: "costmap_2d::StaticLayer"
+    - name: inflation_layer
+      type: "costmap_2d::InflationLayer"
+
+  inflation_layer:
+     inflation_radius: 0.5
+     cost_scaling_factor: 10.0
diff --git a/clober_mbf/config/local_costmap.yaml b/clober_mbf/config/local_costmap.yaml
@@ -0,0 +1,21 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+
+  static_map: false
+  rolling_window: true
+
+  width: 6.0
+  height: 6.0
+
+  footprint_padding: 0.0
+
+  plugins:
+    - name: obstacle_layer
+      type: "costmap_2d::ObstacleLayer"
+    - name: inflation_layer
+      type: "costmap_2d::InflationLayer"
+
+  inflation_layer:
+    inflation_radius: 0.5
+    cost_scaling_factor: 10.0
diff --git a/clober_mbf/config/move_base_flex.yaml b/clober_mbf/config/move_base_flex.yaml
@@ -0,0 +1,12 @@
+controller_frequency: 20.0
+controller_patience: 15.0 
+
+planner_frequency: 1.0
+planner_patience: 5.0
+
+shutdown_costmaps: false
+oscillation_timeout: 0.0
+oscillation_distance: 0.5
+
+recovery_behavior_enabled: true
+clearing_rotation_allowed: false
diff --git a/clober_mbf/config/planners.yaml b/clober_mbf/config/planners.yaml
@@ -0,0 +1,31 @@
+planners:
+# You can add more planners you need to use.
+#- name: ''
+#  type: ''
+ - name: GlobalPlanner
+   type: global_planner/GlobalPlanner
+
+GlobalPlanner:
+  allow_unknown: true                           # Allow planner to plan through unknown space, default true
+  default_tolerance: 0.5                        # If goal in obstacle, plan to the closest point in radius default_tolerance, default 0.0
+  visualize_potential : true
+
+  use_dijkstra: true                            # Use dijkstra's algorithm. Otherwise, A*, default true
+  use_quadratic: true                           # Use the quadratic approximation of the potential. Otherwise, use a simpler calculation, default true
+  use_grid_path: false                          # Create a path that follows the grid boundaries. Otherwise, use a gradient descent method, default false
+  old_navfn_behavior: false                     # Exactly mirror behavior of navfn, use defaults for other boolean parameters, default false
+                                                #Needs to have track_unknown_space: true in the obstacle / voxel layer (in costmap_commons_param) to work
+  lethal_cost: 253                              # default 253
+  neutral_cost: 33 #66                              # default 50
+  cost_factor: 1.0 #0.55                              # Factor to multiply each cost from costmap by, default 3.0
+  publish_potential: true                       # Publish Potential Costmap (this is not like the navfn pointcloud2 potential), default true
+
+  orientation_mode: 0
+  orientation_window_size: 1
+
+  planner_window_x: 0.0                         # default 0.0
+  planner_window_y: 0.0                         # default 0.0
+
+  publish_scale: 100                            # Scale by which the published potential gets multiplied, default 100
+
+
diff --git a/clober_mbf/config/test_local_costmap_params.yaml b/clober_mbf/config/test_local_costmap_params.yaml
@@ -0,0 +1,15 @@
+test_local_costmap:
+  footprint: [[-0.202, -0.202], [-0.202, 0.202], [0.202, 0.202], [0.202, -0.202]] 
+  global_frame: map                 #The global frame for the costmap to operate in.
+  robot_base_frame: base_link       #The name of the frame for the base link of the robot.
+
+  transform_tolerance: 1.0          #Specifies the delay in transform (tf) data that is tolerable in seconds. 
+  update_frequency: 10.0            #The frequency in Hz for the map to be updated.
+  publish_frequency: 10.0           #The frequency in Hz for the map to be publish display information.
+  static_map: false
+  rolling_window: true              #Whether or not to use a rolling window version of the costmap. 
+  width: 6.0                        #The width of the map in meters.
+  height: 6.0                       #The height of the map in meters.                       
+  resolution: 0.05                  #The resolution of the map in meters/cell.
+
+
diff --git a/clober_mbf/launch/include/move_base_flex.launch.xml b/clober_mbf/launch/include/move_base_flex.launch.xml
@@ -0,0 +1,50 @@
+<?xml version="1.0"?>
+<launch>
+
+  <arg name="set_map_frame"  default="map"/>
+  <arg name="set_base_frame" default="base_footprint"/>
+  <arg name="set_scan_frame" default="base_scan"/>
+  <arg name="set_odom_frame" default="odom"/>
+  <arg name='robot_frame' default='base_link'/>
+
+  <arg name="cmd_vel_topic" default="cmd_vel" />
+  <arg name="odom_topic" default="odom" />
+
+  <arg name="base_global_planner" default="GlobalPlanner"/>
+  <arg name="base_local_planner" default="teb"/>
+
+
+  <node name="move_base_flex" pkg="mbf_costmap_nav" type="mbf_costmap_nav">
+    <rosparam file="$(find clober_mbf)/config/move_base_flex.yaml" command="load"/>
+    <rosparam file="$(find clober_mbf)/config/costmap_common.yaml" command="load" ns="global_costmap"/>
+    <rosparam file="$(find clober_mbf)/config/costmap_common.yaml" command="load" ns="local_costmap"/>
+    <rosparam file="$(find clober_mbf)/config/local_costmap.yaml" command="load"/>
+    <rosparam file="$(find clober_mbf)/config/global_costmap.yaml" command="load"/>
+    <rosparam file="$(find clober_mbf)/config/planners.yaml" command="load"/>
+    <rosparam file="$(find clober_mbf)/config/controllers.yaml" command="load"/>
+
+    <param name="global_costmap/global_frame"       value="$(arg set_map_frame)"/>
+    <param name="global_costmap/robot_base_frame"   value="$(arg set_base_frame)"/>
+    <param name="global_costmap/scan/sensor_frame"  value="$(arg set_scan_frame)"/>
+    <param name="global_costmap/obstacle_layer/scan/sensor_frame"   value="$(arg set_scan_frame)"/>
+
+    <param name="local_costmap/global_frame"        value="$(arg set_map_frame)"/>
+    <param name="local_costmap/robot_base_frame"    value="$(arg set_base_frame)"/>
+    <param name="local_costmap/scan/sensor_frame"   value="$(arg set_scan_frame)"/>
+    <param name="local_costmap/obstacle_layer/scan/sensor_frame"   value="$(arg set_scan_frame)"/>
+
+    <param name="robot_frame" value="$(arg set_base_frame)"/>
+
+    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
+    <remap from="odom" to="$(arg odom_topic)"/>
+    <remap from="base_link" to="$(arg set_base_frame)"/>
+    <remap from="map" to="/map"/>
+    <remap from="static_map" to="/static_map"/>
+  </node>
+
+  <node name="move_base_legacy_relay" pkg="mbf_costmap_nav" type="move_base_legacy_relay.py">
+    <param name="base_global_planner" value="$(arg base_global_planner)"/>
+    <param name="base_local_planner" value="$(arg base_local_planner)"/>
+  </node>
+
+</launch>
diff --git a/clober_mbf/launch/navigation.launch b/clober_mbf/launch/navigation.launch
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Arguments -->
+  <arg name="map_file" default="$(find clober_navigation)/maps/map.yaml"/>
+  <arg name="open_rviz" default="true"/>
+
+  <!-- Map server -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)"/>
+
+  <!-- AMCL -->
+  <include file="$(find clober_navigation)/launch/amcl.launch"/>
+
+  <!-- move_base flex -->
+  <include file="$(find clober_mbf)/launch/include/move_base_flex.launch.xml"/>
+
+  <!-- rviz -->
+  <group if="$(arg open_rviz)">
+    <include file="$(find clober_description)/launch/rviz.launch"/>
+  </group>
+</launch>
diff --git a/clober_mbf/package.xml b/clober_mbf/package.xml
@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>clober_mbf</name>
+  <version>1.0.4</version>
+  <description>
+    This package includes launch files for navigation using move_base_flex
+  </description>
+  <maintainer email="[email protected]">clobot-git</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roslint</build_depend>
+
+  <depend>amcl</depend>
+  <depend>move_base</depend>
+  <depend>move_base_flex</depend>
+  <depend>costmap_2d</depend>
+  <depend>costmap_converter</depend>
+  <depend>base_local_planner</depend>
+  <depend>teb_local_planner</depend>
+  <depend>map_server</depend>
+
+</package>
diff --git a/clober_simulation/launch/rmf/rmf_suntech_5x5.launch b/clober_simulation/launch/rmf/rmf_suntech_5x5.launch
@@ -23,7 +23,7 @@
     <arg name="world_name" value="$(find clober_simulation)/worlds/suntech_5x5.world"/>
     <arg name="paused" value="false"/>
     <arg name="use_sim_time" value="true"/>
-    <arg name="gui" value="true"/>
+    <arg name="gui" value="false"/>
     <arg name="headless" value="false"/>
     <arg name="debug" value="false"/>
   </include>