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,99 @@
+#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(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(std::vector<geometry_msgs::PoseStamped>& path);
+
+  bool initialized_;
+  base_local_planner::OdometryHelperRos odom_helper_;
+  tf::TransformListener* tf_;  ///< @brief Used for transforming point clouds
+  costmap_2d::Costmap2DROS* costmap_ros_;
+  tf::Stamped<tf::Pose> current_pose_;
+  ros::Publisher g_plan_pub_, l_plan_pub_;
+  ros::ServiceClient goal_reached_client_, set_plan_client_, initialize_client_,
+      compute_velocity_client_;
+  std::string goal_reached_srv_ = "goal_reached";
+  std::string set_plan_srv_ = "set_plan";
+  std::string initialize_srv_ = "initialize";
+  std::string compute_velocity_srv_ = "compute_velocity";
+
+
+};
+}
+
+#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]">niko</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>

omg_local_planner/src/omg_planner_ros.cpp

@@ -0,0 +1,105 @@
+#include <pluginlib/class_list_macros.h>
+
+#include <omg_local_planner/omg_planner_ros.h>
+
+#include <omg_ros_nav_bridge/GoalReached.h>
+
+// register this planner as a BaseLocalPlanner plugin
+PLUGINLIB_EXPORT_CLASS(omg_local_planner::OMGPlannerROS,
+                       nav_core::BaseLocalPlanner)
+
+namespace omg_local_planner {
+OMGPlannerROS::OMGPlannerROS() : initialized_(false), odom_helper_("odom") {}
+
+void OMGPlannerROS::initialize(std::__cxx11::string name,
+                               tf::TransformListener *tf,
+                               costmap_2d::Costmap2DROS *costmap_ros) {
+  if (!initialized_) {
+    ros::NodeHandle private_nh("~/" + name);
+    g_plan_pub_ = private_nh.advertise<nav_msgs::Path>("global_plan", 1);
+    l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
+    tf_ = tf;
+    costmap_ros_ = costmap_ros;
+    costmap_ros_->getRobotPose(current_pose_);
+
+    // Initialise the service clients.
+    ros::NodeHandle public_nh;
+    ros::service::waitForService(goal_reached_srv_, -1);
+    goal_reached_client_ =
+        public_nh.serviceClient<omg_ros_nav_bridge::GoalReached>(
+            goal_reached_srv_, true);
+    // TODO: Add the other services and call initialise
+    initialized_ = true;
+  }
+}
+
+OMGPlannerROS::~OMGPlannerROS() {}
+
+bool OMGPlannerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) {
+  if (!costmap_ros_->getRobotPose(current_pose_)) {
+    ROS_ERROR("Could not get robot pose");
+    return false;
+  }
+
+  if (compute_velocity_client_) {
+    // TODO: Add call to service
+  } else {
+    ROS_ERROR_STREAM("compute_velocity_client_ is not connected.");
+  }
+}
+
+bool OMGPlannerROS::setPlan(
+    const std::vector<geometry_msgs::PoseStamped> &orig_global_plan) {
+  if (!isInitialized()) {
+    ROS_ERROR(
+        "This planner has not been initialized, please call initialize() "
+        "before using this planner");
+    return false;
+  }
+
+  ROS_INFO("Got new plan");
+  if (set_plan_client_) {
+    // TODO: Add call to service
+  } else {
+    ROS_ERROR_STREAM("set_plan_client_ is not connected.");
+  }
+}
+
+bool OMGPlannerROS::isGoalReached() {
+  if (!isInitialized()) {
+    ROS_ERROR(
+        "This planner has not been initialized, please call initialize() "
+        "before using this planner");
+    return false;
+  }
+
+  if (!costmap_ros_->getRobotPose(current_pose_)) {
+    ROS_ERROR("Could not get robot pose");
+    return false;
+  }
+
+  if (goal_reached_client_) {
+    omg_ros_nav_bridge::GoalReached srv;
+    if (goal_reached_client_.call(srv)) {
+      return srv.response.reached;
+    } else {
+      ROS_ERROR("Failed to call service goal_reached");
+      return 1;
+    }
+  } else {
+    ROS_ERROR_STREAM("goal_reached_client_ is not connected.");
+  }
+}
+
+bool OMGPlannerROS::isInitialized() { return initialized_; }
+
+void OMGPlannerROS::publishLocalPlan(
+    std::vector<geometry_msgs::PoseStamped> &path) {
+  base_local_planner::publishPlan(path, l_plan_pub_);
+}
+
+void OMGPlannerROS::publishGlobalPlan(
+    std::vector<geometry_msgs::PoseStamped> &path) {
+  base_local_planner::publishPlan(path, g_plan_pub_);
+}
+}