[NAV] Added global planner with orientations

mcr_global_planner/CMakeLists.txt

@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mcr_global_planner)
+
+find_package(catkin REQUIRED
+  COMPONENTS
+    costmap_2d
+    dynamic_reconfigure
+    geometry_msgs
+    nav_core
+    navfn
+    nav_msgs
+    pluginlib
+    roscpp
+    tf
+    global_planner
+)
+
+catkin_package(
+  INCLUDE_DIRS 
+     ros/include
+  LIBRARIES 
+     ${PROJECT_NAME}
+  CATKIN_DEPENDS
+    costmap_2d
+    dynamic_reconfigure
+    geometry_msgs
+    nav_core
+    navfn
+    nav_msgs
+    pluginlib
+    roscpp
+    tf
+    global_planner
+)
+
+include_directories(
+  ros/include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(
+  ${PROJECT_NAME} ros/src/${PROJECT_NAME}/global_planner_wo.cpp
+)
+target_link_libraries(
+  ${PROJECT_NAME} ${catkin_LIBRARIES}
+)
+
+add_dependencies(
+  ${PROJECT_NAME} ${PROJECT_NAME}_gencfg
+)
+
+
+install(TARGETS ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+
+install(FILES plugins.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)

mcr_global_planner/package.xml

@@ -0,0 +1,40 @@
+<package>
+  <name>mcr_global_planner</name>
+  <version>1.0.0</version>
+  <description>
+     Collection of b-it-bots global planners.
+  </description>
+  <maintainer email="[email protected]">Torsten Jandt</maintainer>
+  <license>GPLv3</license>
+
+  <author email="[email protected]">Torsten Jandt</author>
+  <url></url>
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>costmap_2d</build_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_core</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>navfn</build_depend>
+  <build_depend>pluginlib</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>tf</build_depend>
+  <build_depend>global_planner</build_depend>
+
+  <run_depend>costmap_2d</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>nav_core</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>navfn</run_depend>
+  <run_depend>pluginlib</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>tf</run_depend>
+  <run_depend>global_planner</run_depend>
+  <export>
+      <nav_core plugin="${prefix}/plugins.xml" />
+  </export>
+
+</package>

mcr_global_planner/plugins.xml

@@ -0,0 +1,7 @@
+<library path="lib/libmcr_global_planner">
+    <class name="mcr_global_planner/GlobalPlannerWO" type="mcr_global_planner::GlobalPlannerWO" base_class_type="nav_core::BaseGlobalPlanner">
+        <description>
+            Wraps ros global planner and modifies the plan by removing poses and adding orientations.
+        </description>
+    </class>
+</library>

mcr_global_planner/ros/include/mcr_global_planner/global_planner_wo.h

@@ -0,0 +1,84 @@
+#ifndef _GLOBALPLANNERWO_H
+#define _GLOBALPLANNERWO_H
+
+/********************************************************************
+ * Copyright [2015] <Bonn-Rhein-Sieg University>
+ * Author: Torsten Jandt
+ *         [email protected]
+ *********************************************************************/
+
+#include <global_planner/planner_core.h>
+
+namespace mcr_global_planner
+{
+class GlobalPlannerWO : public global_planner::GlobalPlanner
+{
+    private:
+        // Minimun distance between two poses.
+        double pose_distance_;
+        // Maximum amount of poses that should be driven in onmi mode only
+        int omni_poses_;
+        // Publisher used to publish the pose-array
+        ros::Publisher pub_plan;
+        bool initialized;
+
+        /*
+         * Filters the plan by removing poses and adds orientations to remaining ones.
+         */
+        void filterPlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
+                        std::vector< geometry_msgs::PoseStamped > &plan);
+        /*
+         * Initialization for this wrapper class.
+         */
+        void initializeThis(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id);
+
+        /*
+         * Used to publish the plan with orientations as PoseArray
+         */
+        void publishVisualPlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
+                               std::vector< geometry_msgs::PoseStamped > &plan);
+
+        /*
+         * Calculates the distance between two poses.
+         */
+        double calcDistance(const geometry_msgs::Pose &pose1, const geometry_msgs::Pose &pose2);
+        double calcDistance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2);
+
+        /*
+         * Calculates the rotation from pose to another
+         */
+        double calcRotation(const geometry_msgs::Pose &from, const geometry_msgs::Pose &to);
+        double calcRotation(const geometry_msgs::PoseStamped &from, const geometry_msgs::PoseStamped &to);
+
+        /*
+         * Calculates the rotation using the right rotation direction with precalced values.
+         */
+        double calcOmniRotation(const double from, const double way1, const double way2, const double weight);
+
+        /*
+         * Calculates the rotation using the right rotation direction.
+         */
+        double calcOmniRotation(const double from, const double to, const double weight);
+
+        /*
+         * mod like helper method
+         */
+        double modlike(double number, double limit);
+
+    public:
+        GlobalPlannerWO();
+        GlobalPlannerWO(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id);
+
+        void initialize(std::string name, costmap_2d::Costmap2DROS *costmap_ros);
+        void initialize(std::string name, costmap_2d::Costmap2D *costmap, std::string frame_id);
+
+        bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal,
+                      std::vector< geometry_msgs::PoseStamped > &plan);
+        bool makePlan(const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, double tolerance,
+                      std::vector< geometry_msgs::PoseStamped > &plan);
+};
+}
+
+#endif
+
+