Create omg local planner

omg_local_planner/CMakeLists.txt

@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(omg_local_planner)
+
+add_compile_options(-std=c++11)
+
+find_package(catkin REQUIRED COMPONENTS
+  base_local_planner
+  costmap_2d
+  geometry_msgs
+  nav_core
+  nav_msgs
+  omg_ros_nav_bridge
+  pluginlib
+  roscpp
+  std_msgs
+  tf
+  rosconsole
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES omg_local_planner
+  CATKIN_DEPENDS geometry_msgs nav_msgs omg_ros_nav_bridge pluginlib roscpp
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+)
+
+add_library(omg_local_planner src/omg_planner_ros.cpp)
+add_dependencies(omg_local_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(omg_local_planner ${catkin_LIBRARIES})
+
+install(TARGETS omg_local_planner
+       ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+       LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+       )
+
+install(FILES blp_plugin.xml
+    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+  PATTERN ".svn" EXCLUDE
+)

omg_local_planner/blp_plugin.xml

@@ -0,0 +1,7 @@
+<library path="lib/libomg_local_planner">
+  <class name="omg_local_planner/OMGPlannerROS" type="omg_local_planner::OMGPlannerROS" base_class_type="nav_core::BaseLocalPlanner">
+    <description>
+      Implementation of a local planner using omg-tools.
+    </description>
+  </class>
+</library>

omg_local_planner/include/omg_local_planner/omg_planner.h

@@ -0,0 +1,12 @@
+#ifndef OMG_LOCAL_PLANNER_OMG_PLANNER_H_
+#define OMG_LOCAL_PLANNER_OMG_PLANNER_H_
+
+namespace omg_local_planner {
+  /**
+   * @class OMGPlanner
+   * @brief A class implementing a local planner using omg-tools
+   */
+  class OMGPlanner {};
+}
+#endif //OMG_LOCAL_PLANNER_OMG_PLANNER_H_
+

omg_local_planner/include/omg_local_planner/omg_planner_ros.h

@@ -0,0 +1,145 @@
+#ifndef OMG_LOCAL_PLANNER_OMG_PLANNER_ROS_H
+#define OMG_LOCAL_PLANNER_OMG_PLANNER_ROS_H
+
+#include <base_local_planner/goal_functions.h>
+#include <base_local_planner/odometry_helper_ros.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/Twist.h>
+#include <nav_core/base_local_planner.h>
+#include <nav_msgs/Path.h>
+#include <tf/transform_listener.h>
+
+#include <string>
+#include <vector>
+
+namespace omg_local_planner {
+/**
+ * @class OMGPlannerROS
+ * @brief ROS Wrapper for the omg-tools planner that adheres to the
+ * BaseLocalPlanner interface and can be used as a plugin for move_base.
+ */
+class OMGPlannerROS : public nav_core::BaseLocalPlanner {
+ public:
+  /**
+   * @brief Constructor for the OMGPlannerROS wrapper.
+   */
+  OMGPlannerROS();
+
+  /**
+   * @brief Initialize Initialises the wrapper.
+   * @param name The name of the trajectory planner instance.
+   * @param tf A pointer to the transform listener.
+   * @param costmap_ros The cost map to use for assigning costs to trajectories.
+   */
+  void initialize(std::string name, tf::TransformListener* tf,
+                  costmap_2d::Costmap2DROS* costmap_ros);
+  /**
+   * @brief Destructor of the wrapper.
+   */
+  ~OMGPlannerROS();
+
+  /**
+   * @brief Given the current position, orientation, and velocity of the robot,
+   * compute velocity commands to send to the base.
+   * @param cmd_vel The velocity command to be passed to the robot base.
+   * @return True if a valid trajectory was found, false otherwise.
+   */
+  bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel);
+
+  /**
+   * @brief setPlan Set the plan that the planner is following.
+   * @param orig_global_plan The plan to pass to the planner.
+   * @return True if the plan was updated successfully, false otherwise.
+   */
+  bool setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan);
+
+  /**
+   * @brief Check if the goal pose has been achieved.
+   * @return True if achieved, false otherwise.
+   */
+  bool isGoalReached();
+
+  /**
+   * @brief Check if the local planner is initialized.
+   * @return True if initialized, false otherwise.
+   */
+  bool isInitialized();
+
+ private:
+  /**
+   * @brief Publish the local plan to be followed.
+   * @param path A set of poses composing the plan.
+   */
+  void publishLocalPlan(const std::vector<geometry_msgs::PoseStamped> &path);
+
+  /**
+   * @brief Publish the global plan to be followed.
+   * @param path A set of poses composing the plan.
+   */
+  void publishGlobalPlan(const std::vector<geometry_msgs::PoseStamped>& path);
+
+  bool initialized_;  ///< @brief Holds the planner initialization status.
+
+  /**
+   * @brief odom_helper_ Class used to provide odometry information.
+   */
+  base_local_planner::OdometryHelperRos odom_helper_;
+
+  tf::TransformListener* tf_;  ///< @brief Used for transforming point clouds.
+
+  costmap_2d::Costmap2DROS* costmap_ros_;  ///< @brief The global costmap.
+  tf::Stamped<tf::Pose>
+      current_pose_;  ///< @brief The current pose of the robot.
+  ros::Publisher g_plan_pub_; ///< @brief Global plan publisher.
+  ros::Publisher l_plan_pub_; ///< @brief Local plan publisher.
+
+
+  /**
+   * @brief goal_reached_client_ Client for calling the goal_reached service.
+   */
+  ros::ServiceClient goal_reached_client_;
+
+  /**
+   * @brief goal_reached_client_ Client for calling the set_plan service.
+   */
+  ros::ServiceClient set_plan_client_;
+
+  /**
+   * @brief goal_reached_client_ Client for calling the initialize service.
+   */
+  ros::ServiceClient initialize_client_;
+
+  /**
+   * @brief goal_reached_client_ Client for calling the compute_velocity service.
+   */
+  ros::ServiceClient compute_velocity_client_;
+
+  /**
+   * @brief goal_reached_srv_ Service name.
+   */
+  const std::string kGoalReachedSrv_ = "goal_reached";
+
+  /**
+   * @brief goal_reached_srv_ Service name.
+   */
+  const std::string kSetPlanSrv_ = "config_planner";
+
+  /**
+   * @brief goal_reached_srv_ Service name.
+   */
+  const std::string kInitializeSrv_ = "init_planner";
+
+  /**
+   * @brief goal_reached_srv_ Service name.
+   */
+  const std::string kComputeVelocitySrv_ = "compute_vel_cmd";
+
+  /**
+   * @brief local_plan_ Store local plan for visualisation.
+   */
+  std::vector<geometry_msgs::PoseStamped> local_plan_;
+};
+}
+
+#endif  // OMG_LOCAL_PLANNER_OMG_PLANNER_ROS_H

omg_local_planner/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>omg_local_planner</name>
+  <version>0.0.0</version>
+  <description>The omg_local_planner package</description>
+
+  <maintainer email="[email protected]">Intermodalics BVBA</maintainer>
+
+
+  <license>Closed</license>
+
+
+  <author email="[email protected]">Nikolaos Tsiogkas</author> 
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <depend>base_local_planner</depend>
+  <depend>costmap_2d</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_core</depend>
+  <depend>nav_msgs</depend>
+  <depend>omg_ros_nav_bridge</depend>
+  <depend>pluginlib</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+  <depend>rosconsole</depend>
+
+  <export>
+    <nav_core plugin="${prefix}/blp_plugin.xml" />
+  </export>
+</package>