Plugin for informed_RRT*

CMakeLists.txt

@@ -14,6 +14,8 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
+  rviz_visual_tools
+  nav_core
 )
 
 ## System dependencies are found with CMake's conventions
@@ -108,8 +110,8 @@ add_definitions(-std=c++11)
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES rrt_star
-  CATKIN_DEPENDS costmap_2d roscpp rospy std_msgs
+#  LIBRARIES rrt_star
+#  CATKIN_DEPENDS costmap_2d roscpp rospy std_msgs rviz_visual_tools base_local_planner
   DEPENDS OMPL
 )
 
@@ -126,14 +128,15 @@ include_directories(
 )
 
 ## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/rrt_star.cpp
-# )
+add_library(${PROJECT_NAME} src/costmap_model.cpp)
+add_library(rrt_lib 
+	src/rrt.cpp
+	src/costmap_model.cpp)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
 ## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+#add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
@@ -148,7 +151,7 @@ add_executable(optimal_planning_node src/OptimalPlanning.cpp)
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+#add_dependencies(optimal_planning_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 link_libraries(${OMPL_LIBRARIES_DIRS})
 
@@ -158,6 +161,11 @@ target_link_libraries(optimal_planning_node
    ${OMPL_LIBRARIES}
 )
 
+target_link_libraries(rrt_lib
+   ${catkin_LIBRARIES}
+   ${OMPL_LIBRARIES}
+)
+
 #############
 ## Install ##
 #############
@@ -173,18 +181,18 @@ target_link_libraries(optimal_planning_node
 # )
 
 ## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#install(TARGETS ${PROJECT_NAME} #${PROJECT_NAME}_node
 #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
+#)
 
 ## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+   #FILES_MATCHING PATTERN "*.h"
+   PATTERN ".svn" EXCLUDE
+)
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
 # install(FILES

launch/rrt.launch

@@ -2,9 +2,16 @@
 	<node name="map_server" pkg="map_server" type="map_server" args="$(find rrt_star)/map/maze.yaml">
 		<param name="frame_id" value="/map"/>
 	</node>
-<!--	<node name="rrt" pkg="rrt_star" type="optimal_planning_node" launch-prefix="valgrind" >
-		<rosparam file="$(find rrt_star)/params/costmap.yaml" command="load" ns="global_costmap" />	
+	<node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0.0 0.0 0.0 0 0 0 1 /map /odom 100"/>
+	<node pkg="tf" type="static_transform_publisher" name="odom_to_base_link" args="2.0 2.0 0.0 0 0 0 1 /odom /base_link 100"/>
+
+	<node name="rrt" pkg="rrt_star" type="optimal_planning_node" output="screen"  >
+		<rosparam file="$(find rrt_star)/params/costmap.yaml" command="load" />
+		<rosparam file="$(find rrt_star)/params/plan.yaml" command="load" />	
+		<remap from="/goal" to="/move_base_simple/goal" />	
 	</node>	
--->
+
+	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find rrt_star)/params/rrt_rviz.rviz" />
+
 </launch>
 

package.xml

@@ -34,15 +34,22 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>rviz_visual_tools</build_depend>
+  <build_depend>base_local_planner</build_depend>
+  <build_depend>nav_core</build_depend>
   <run_depend>costmap_2d</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>rviz_visual_tools</run_depend>
+  <run_depend>base_local_planner</run_depend>
+  <run_depend>nav_core</run_depend>
 
+  <run_depend>teb_local_planner</run_depend>
+  <run_depend>move_base</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <!-- Other tools can request additional information be placed here -->
-
+    <nav_core plugin="${prefix}/rrt_plugin.xml"/>
   </export>
 </package>