Mgg planner refactor.

MISTLab · Jan 17, 2025 · f007e3d · f007e3d
1 parent e024ddc
commit f007e3d
Show file tree

Hide file tree

Showing 200 changed files with 272,975 additions and 536 deletions.
diff --git a/README.md b/README.md
@@ -33,23 +33,23 @@ ros-noetic-octomap-ros
 
 Create the workspace:
 ```bash
-mkdir -p gbplanner2_ws/src/exploration
-cd gbplanner2_ws/src/exploration
+mkdir -p mggplanner_ws/src/exploration
+cd mggplanner_ws/src/exploration
 ```
 Clone the planner
 ```bash
-git clone http://git.mistlab.ca/vvaradharajan/gbplanner2.git -b gbplanner2
+git clone http://git.mistlab.ca/vvaradharajan/gbplanner2.git -b mggplanner
 ```
 
 Clone and update the required packages:
 ```bash
-cd <path/to/gbplanner2_ws>
+cd <path/to/mggplanner_ws>
 wstool init
-wstool merge ./src/exploration/gbplanner2/packages_https.rosinstall
+wstool merge ./src/exploration/mggplanner/packages_https.rosinstall
 wstool update
 ```
 
-`Note: ./src/exploration/gbplanner_ros/packages_https.rosinstall can be used for https based urls.`
+`Note: ./src/exploration/mggplanner_ros/packages_https.rosinstall can be used for https based urls.`
 
 Build:
 ```bash
@@ -61,14 +61,14 @@ catkin build
 ### Aerial Robot Demo
 Download the gazebo model from [here](https://ntnu.box.com/s/45c4kb9ywr1kckhkxnzyd0vrcbn1w176) and extract in the `~/.gazebo/models` folder.
 ```bash
-roslaunch gbplanner rmf_sim.launch
+roslaunch mggplanner rmf_sim.launch
 ```
 It will take few moments to load the world. A message saying the spawn_rmf_obelix process has died may pop up, but as long as the pointcloud map is visible in rviz and the UI controls work this message can be safely ignored.
 
 ### Ground Robot Demo
 the following command:
 ```bash
-roslaunch gbplanner smb_sim.launch
+roslaunch mggplanner smb_sim.launch
 ```
 In Ubuntu 18.04 with ROS Melodic, the gazebo node might crash when running the ground robot simulation. In this case set the `gpu` parameter to false [here](https://github.com/ntnu-arl/smb_simulator/blob/6ed9d738ffd045d666311a8ba266570f58dca438/smb_description/urdf/sensor_head.urdf.xacro#L20).
 

diff --git a/gbplanner_ui/launch/gbplanner_ui.launch b/gbplanner_ui/launch/gbplanner_ui.launch
diff --git a/gbplanner_ui/plugin_description.xml b/gbplanner_ui/plugin_description.xml
diff --git a/local_planner/CMakeLists.txt b/local_planner/CMakeLists.txt
@@ -0,0 +1,75 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(local_planner)
+
+set(CMAKE_BUILD_TYPE Release)
+set(BUILD_STATIC_LIBS ON)
+set(BUILD_SHARED_LIBS OFF)
+set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
+
+## Find catkin macros and libraries
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  std_msgs
+  sensor_msgs
+  pcl_ros
+)
+
+find_package(PCL REQUIRED)
+
+###################################
+## catkin specific configuration ##
+###################################
+
+catkin_package(
+  CATKIN_DEPENDS
+  roscpp
+  std_msgs
+  sensor_msgs
+  pcl_ros
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  "${PROJECT_SOURCE_DIR}/include"
+  /usr/local/include # Location when using 'make system_install'
+  /usr/include       # More usual location (e.g. when installing using a package)
+)
+
+## Specify additional locations for library files
+link_directories(
+  /usr/local/lib # Location when using 'make system_install'
+  /usr/lib       # More usual location (e.g. when installing using a package)
+)
+
+## Declare executables
+add_executable(localPlanner src/localPlanner.cpp)
+add_executable(pathFollower src/pathFollower.cpp)
+add_executable(localPlanner_gb src/localPlanner_gb.cpp)
+add_executable(pathFollower_gb src/pathFollower_gb.cpp)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(localPlanner ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+target_link_libraries(pathFollower ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+
+target_link_libraries(localPlanner_gb ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+target_link_libraries(pathFollower_gb ${catkin_LIBRARIES} ${PCL_LIBRARIES})
+
+install(TARGETS localPlanner pathFollower
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY launch/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
+)
+install(DIRECTORY paths/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/paths
+)
diff --git a/local_planner/launch/local_planner.launch b/local_planner/launch/local_planner.launch
@@ -0,0 +1,91 @@
+<launch>
+
+  <arg name="sensorOffsetX" default="0"/>
+  <arg name="sensorOffsetY" default="0"/>
+  <arg name="cameraOffsetZ" default="0"/>
+  <arg name="twoWayDrive" default="true"/>
+  <arg name="maxSpeed" default="1.0"/>
+  <arg name="autonomyMode" default="true"/>
+  <arg name="autonomySpeed" default="0.5"/>
+  <arg name="joyToSpeedDelay" default="2.0"/>
+  <arg name="goalX" default="0"/>
+  <arg name="goalY" default="0"/>
+
+  <node pkg="local_planner" type="localPlanner" name="localPlanner" output="screen" required="true">
+    <param name="pathFolder" type="string" value="$(find local_planner)/paths" />
+    <param name="vehicleLength" type="double" value="0.6" />
+    <param name="vehicleWidth" type="double" value="0.6" />
+    <param name="sensorOffsetX" value="$(arg sensorOffsetX)" />
+    <param name="sensorOffsetY" value="$(arg sensorOffsetY)" />
+    <param name="twoWayDrive" value="$(arg twoWayDrive)" />
+    <param name="laserVoxelSize" type="double" value="0.05" />
+    <param name="terrainVoxelSize" type="double" value="0.2" />
+    <param name="useTerrainAnalysis" type="bool" value="true" />
+    <param name="checkObstacle" type="bool" value="true" />
+    <param name="checkRotObstacle" type="bool" value="false" />
+    <param name="adjacentRange" type="double" value="4.25" />
+    <param name="obstacleHeightThre" type="double" value="0.15" />
+    <param name="groundHeightThre" type="double" value="0.1" />
+    <param name="costHeightThre" type="double" value="0.1" />
+    <param name="costScore" type="double" value="0.02" />
+    <param name="useCost" type="bool" value="false" />
+    <param name="pointPerPathThre" type="int" value="2" />
+    <param name="minRelZ" type="double" value="-0.5" />
+    <param name="maxRelZ" type="double" value="0.25" />
+    <param name="maxSpeed" value="$(arg maxSpeed)" />
+    <param name="dirWeight" type="double" value="0.02" />
+    <param name="dirThre" type="double" value="90.0" />
+    <param name="dirToVehicle" type="bool" value="false" />
+    <param name="pathScale" type="double" value="1.25" />
+    <param name="minPathScale" type="double" value="0.75" />
+    <param name="pathScaleStep" type="double" value="0.25" />
+    <param name="pathScaleBySpeed" type="bool" value="true" />
+    <param name="minPathRange" type="double" value="1.0" />
+    <param name="pathRangeStep" type="double" value="0.5" />
+    <param name="pathRangeBySpeed" type="bool" value="true" />
+    <param name="pathCropByGoal" type="bool" value="true" />
+    <param name="autonomyMode" value="$(arg autonomyMode)" />
+    <param name="autonomySpeed" value="$(arg autonomySpeed)" />
+    <param name="joyToSpeedDelay" value="$(arg joyToSpeedDelay)" />
+    <param name="joyToCheckObstacleDelay" type="double" value="5.0" />
+    <param name="goalClearRange" type="double" value="0.5" />
+    <param name="goalX" type="double" value="$(arg goalX)" />
+    <param name="goalY" type="double" value="$(arg goalY)" />
+  </node>
+
+  <node pkg="local_planner" type="pathFollower" name="pathFollower" output="screen" required="true">
+    <param name="sensorOffsetX" value="$(arg sensorOffsetX)" />
+    <param name="sensorOffsetY" value="$(arg sensorOffsetY)" />
+    <param name="pubSkipNum" type="int" value="1" />
+    <param name="twoWayDrive" value="$(arg twoWayDrive)" />
+    <param name="lookAheadDis" type="double" value="0.5" />
+    <param name="yawRateGain" type="double" value="7.5" />
+    <param name="stopYawRateGain" type="double" value="7.5" />
+    <param name="maxYawRate" type="double" value="90.0" />
+    <param name="maxSpeed" value="$(arg maxSpeed)" />
+    <param name="maxAccel" type="double" value="2.5" />
+    <param name="switchTimeThre" type="double" value="1.0" />
+    <param name="dirDiffThre" type="double" value="0.1" />
+    <param name="stopDisThre" type="double" value="0.2" />
+    <param name="slowDwnDisThre" type="double" value="0.85" />
+    <param name="useInclRateToSlow" type="bool" value="false" />
+    <param name="inclRateThre" type="double" value="120.0" />
+    <param name="slowRate1" type="double" value="0.25" />
+    <param name="slowRate2" type="double" value="0.5" />
+    <param name="slowTime1" type="double" value="2.0" />
+    <param name="slowTime2" type="double" value="2.0" />
+    <param name="useInclToStop" type="bool" value="false" />
+    <param name="inclThre" type="double" value="45.0" />
+    <param name="stopTime" type="double" value="5.0" />
+    <param name="noRotAtStop" type="bool" value="false" />
+    <param name="noRotAtGoal" type="bool" value="true" />
+    <param name="autonomyMode" value="$(arg autonomyMode)" />
+    <param name="autonomySpeed" value="$(arg autonomySpeed)" />
+    <param name="joyToSpeedDelay" value="$(arg joyToSpeedDelay)" />
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher" name="vehicleTransPublisher" args="-$(arg sensorOffsetX) -$(arg sensorOffsetY) 0 0 0 0 /sensor /vehicle 1000"/>
+
+  <node pkg="tf" type="static_transform_publisher" name="sensorTransPublisher" args="0 0 $(arg cameraOffsetZ) -1.5707963 0 -1.5707963 /sensor /camera 1000"/>
+
+</launch>
diff --git a/local_planner/launch/local_planner_mggplanner.launch b/local_planner/launch/local_planner_mggplanner.launch
@@ -0,0 +1,106 @@
+<launch>
+
+  <arg name="sensorOffsetX" default="0"/>
+  <arg name="sensorOffsetY" default="0"/>
+  <arg name="cameraOffsetZ" default="0"/>
+  <arg name="twoWayDrive" default="true"/>
+  <arg name="maxSpeed" default="1.0"/>
+  <arg name="autonomyMode" default="true"/>
+  <arg name="autonomySpeed" default="0.5"/>
+  <arg name="joyToSpeedDelay" default="2.0"/>
+  <arg name="goalX" default="0"/>
+  <arg name="goalY" default="0"/>
+  <arg name="RobotID_TF_prefix" default=""/>
+
+  <node pkg="local_planner" type="localPlanner_gb" name="localPlanner" output="screen" required="true">
+    <remap from="cmd_vel" to="smb_velocity_controller/cmd_vel" />
+		<remap from="state_estimation" to="ground_truth/state" />
+		<remap from="registered_scan" to="mggplanner_node/surface_pointcloud" />
+		<remap from="way_point" to="way_point" />
+    <param name="pathFolder" type="string" value="$(find local_planner)/paths" />
+    <param name="vehicleLength" type="double" value="0.5" />
+    <param name="vehicleWidth" type="double" value="0.5" />
+    <param name="sensorOffsetX" value="$(arg sensorOffsetX)" />
+    <param name="sensorOffsetY" value="$(arg sensorOffsetY)" />
+    <param name="twoWayDrive" value="$(arg twoWayDrive)" />
+    <param name="laserVoxelSize" type="double" value="0.05" />
+    <param name="terrainVoxelSize" type="double" value="0.2" />
+    <param name="useTerrainAnalysis" type="bool" value="false" />
+    <param name="checkObstacle" type="bool" value="true" />
+    <param name="checkRotObstacle" type="bool" value="false" />
+    <param name="adjacentRange" type="double" value="4.25" />
+    <param name="obstacleHeightThre" type="double" value="0.15" />
+    <param name="groundHeightThre" type="double" value="0.1" />
+    <param name="costHeightThre" type="double" value="0.1" />
+    <param name="costScore" type="double" value="0.02" />
+    <param name="useCost" type="bool" value="false" />
+    <param name="pointPerPathThre" type="int" value="2" />
+    <param name="minRelZ" type="double" value="0.0" />
+    <param name="maxRelZ" type="double" value="1.0" />
+    <param name="maxSpeed" value="$(arg maxSpeed)" />
+    <param name="dirWeight" type="double" value="0.02" />
+    <param name="dirThre" type="double" value="90.0" />
+    <param name="dirToVehicle" type="bool" value="false" />
+    <param name="pathScale" type="double" value="0.65" />
+    <param name="minPathScale" type="double" value="0.65" />
+    <param name="pathScaleStep" type="double" value="0.25" />
+    <param name="pathScaleBySpeed" type="bool" value="true" />
+    <param name="minPathRange" type="double" value="0.75" />
+    <param name="pathRangeStep" type="double" value="0.2" />
+    <param name="pathRangeBySpeed" type="bool" value="true" />
+    <param name="pathCropByGoal" type="bool" value="true" />
+    <param name="autonomyMode" value="$(arg autonomyMode)" />
+    <param name="autonomySpeed" value="$(arg autonomySpeed)" />
+    <param name="joyToSpeedDelay" value="$(arg joyToSpeedDelay)" />
+    <param name="joyToCheckObstacleDelay" type="double" value="5.0" />
+    <param name="goalClearRange" type="double" value="0.5" />
+    <param name="goalX" type="double" value="$(arg goalX)" />
+    <param name="goalY" type="double" value="$(arg goalY)" />
+    <param name="RobotID_TF_prefix" type="string" value="$(arg RobotID_TF_prefix)" />
+
+  </node>
+
+  <node pkg="local_planner" type="pathFollower_gb" name="pathFollower" output="screen" required="true">
+    <remap from="cmd_vel" to="smb_velocity_controller/cmd_vel" />
+		<remap from="state_estimation" to="ground_truth/state" />
+		<remap from="registered_scan" to="mggplanner_node/surface_pointcloud" />
+		<remap from="way_point" to="way_point" />
+    <param name="sensorOffsetX" value="$(arg sensorOffsetX)" />
+    <param name="sensorOffsetY" value="$(arg sensorOffsetY)" />
+    <param name="pubSkipNum" type="int" value="1" />
+    <param name="twoWayDrive" value="$(arg twoWayDrive)" />
+    <param name="lookAheadDis" type="double" value="0.5" />
+    <param name="yawRateGain" type="double" value="7.5" />
+    <param name="stopYawRateGain" type="double" value="7.5" />
+    <param name="maxYawRate" type="double" value="90.0" />
+    <param name="maxSpeed" value="$(arg maxSpeed)" />
+    <param name="maxAccel" type="double" value="2.5" />
+    <param name="switchTimeThre" type="double" value="1.0" />
+    <param name="dirDiffThre" type="double" value="0.1" />
+    <param name="stopDisThre" type="double" value="0.2" />
+    <param name="slowDwnDisThre" type="double" value="0.85" />
+    <param name="useInclRateToSlow" type="bool" value="false" />
+    <param name="inclRateThre" type="double" value="120.0" />
+    <param name="slowRate1" type="double" value="0.25" />
+    <param name="slowRate2" type="double" value="0.5" />
+    <param name="slowTime1" type="double" value="2.0" />
+    <param name="slowTime2" type="double" value="2.0" />
+    <param name="useInclToStop" type="bool" value="false" />
+    <param name="inclThre" type="double" value="45.0" />
+    <param name="stopTime" type="double" value="5.0" />
+    <param name="noRotAtStop" type="bool" value="false" />
+    <param name="noRotAtGoal" type="bool" value="true" />
+    <param name="autonomyMode" value="$(arg autonomyMode)" />
+    <param name="autonomySpeed" value="$(arg autonomySpeed)" />
+    <param name="joyToSpeedDelay" value="$(arg joyToSpeedDelay)" />
+    <param name="RobotID_TF_prefix" type="string" value="$(arg RobotID_TF_prefix)" />
+  </node>
+
+
+
+
+  <!-- <node pkg="tf" type="static_transform_publisher" name="vehicleTransPublisher" args="-$(arg sensorOffsetX) -$(arg sensorOffsetY) 0 0 0 0 /sensor /vehicle 1000"/> -->
+
+  <!-- <node pkg="tf" type="static_transform_publisher" name="sensorTransPublisher" args="0 0 $(arg cameraOffsetZ) -1.5707963 0 -1.5707963 /sensor /camera 1000"/> -->
+
+</launch>
diff --git a/local_planner/package.xml b/local_planner/package.xml
@@ -0,0 +1,21 @@
+<package>
+  <name>local_planner</name>
+  <version>0.0.1</version>
+  <description>Local Planner</description>
+  <maintainer email="[email protected]">Ji Zhang</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>message_filters</build_depend>
+  <build_depend>pcl_ros</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>message_filters</run_depend>
+  <run_depend>pcl_ros</run_depend>
+</package>