diff --git a/omg_local_planner/CMakeLists.txt b/omg_local_planner/CMakeLists.txt
new file mode 100644
index 0000000000..8ebeb9a123
--- /dev/null
+++ b/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
+)
diff --git a/omg_local_planner/blp_plugin.xml b/omg_local_planner/blp_plugin.xml
new file mode 100644
index 0000000000..653ae8497e
--- /dev/null
+++ b/omg_local_planner/blp_plugin.xml
@@ -0,0 +1,7 @@
+
+
+
+ Implementation of a local planner using omg-tools.
+
+
+
diff --git a/omg_local_planner/include/omg_local_planner/omg_planner.h b/omg_local_planner/include/omg_local_planner/omg_planner.h
new file mode 100644
index 0000000000..edf9120277
--- /dev/null
+++ b/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_
+
diff --git a/omg_local_planner/include/omg_local_planner/omg_planner_ros.h b/omg_local_planner/include/omg_local_planner/omg_planner_ros.h
new file mode 100644
index 0000000000..080891675f
--- /dev/null
+++ b/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
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+
+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& 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 &path);
+
+ /**
+ * @brief Publish the global plan to be followed.
+ * @param path A set of poses composing the plan.
+ */
+ void publishGlobalPlan(const std::vector& 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
+ 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 local_plan_;
+};
+}
+
+#endif // OMG_LOCAL_PLANNER_OMG_PLANNER_ROS_H
diff --git a/omg_local_planner/package.xml b/omg_local_planner/package.xml
new file mode 100644
index 0000000000..f7e3ba50e4
--- /dev/null
+++ b/omg_local_planner/package.xml
@@ -0,0 +1,31 @@
+
+
+ omg_local_planner
+ 0.0.0
+ The omg_local_planner package
+
+ Intermodalics BVBA
+
+
+ Closed
+
+
+ Nikolaos Tsiogkas
+
+ catkin
+ base_local_planner
+ costmap_2d
+ geometry_msgs
+ nav_core
+ nav_msgs
+ omg_ros_nav_bridge
+ pluginlib
+ roscpp
+ std_msgs
+ tf
+ rosconsole
+
+
+
+
+
diff --git a/omg_local_planner/src/omg_planner.cpp b/omg_local_planner/src/omg_planner.cpp
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp
new file mode 100644
index 0000000000..30b14c47a5
--- /dev/null
+++ b/omg_local_planner/src/omg_planner_ros.cpp
@@ -0,0 +1,214 @@
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+
+// 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::string name,
+ tf::TransformListener *tf,
+ costmap_2d::Costmap2DROS *costmap_ros) {
+ if (!initialized_) {
+ ros::NodeHandle private_nh("~/" + name);
+ g_plan_pub_ = private_nh.advertise("global_plan", 1);
+ l_plan_pub_ = private_nh.advertise("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(kGoalReachedSrv_, -1);
+ goal_reached_client_ =
+ public_nh.serviceClient(
+ kGoalReachedSrv_, true);
+ set_plan_client_ =
+ public_nh.serviceClient(kSetPlanSrv_,
+ true);
+ initialize_client_ =
+ public_nh.serviceClient(
+ kInitializeSrv_, true);
+ compute_velocity_client_ =
+ public_nh.serviceClient(
+ kComputeVelocitySrv_, true);
+
+ omg_ros_nav_bridge::InitPlanner srv;
+ {
+ // TODO: This assumes that the costmap is static.
+ // In the ros publisher they check if it has moved.
+ // Should check that this is not the case.
+ char* cost_translation_table_ = new char[256];
+
+ // special values:
+ cost_translation_table_[0] = 0; // NO obstacle
+ cost_translation_table_[253] = 99; // INSCRIBED obstacle
+ cost_translation_table_[254] = 100; // LETHAL obstacle
+ cost_translation_table_[255] = -1; // UNKNOWN
+
+ // regular cost values scale the range 1 to 252 (inclusive) to fit
+ // into 1 to 98 (inclusive).
+ for (int i = 1; i < 253; i++) {
+ cost_translation_table_[ i ] = char(1 + (97 * (i - 1)) / 251);
+ }
+
+ boost::unique_lock
+ lock(*(costmap_ros_->getCostmap()->getMutex()));
+ double resolution = costmap_ros_->getCostmap()->getResolution();
+
+ srv.request.global_costmap.header.frame_id =
+ costmap_ros_->getGlobalFrameID();
+ srv.request.global_costmap.header.stamp = ros::Time::now();
+ srv.request.global_costmap.info.resolution = resolution;
+
+ srv.request.global_costmap.info.width =
+ costmap_ros_->getCostmap()->getSizeInCellsX();
+ srv.request.global_costmap.info.height =
+ costmap_ros_->getCostmap()->getSizeInCellsY();
+
+ double wx, wy;
+ costmap_ros_->getCostmap()->mapToWorld(0, 0, wx, wy);
+ srv.request.global_costmap.info.origin.position.x = wx - resolution / 2;
+ srv.request.global_costmap.info.origin.position.y = wy - resolution / 2;
+ srv.request.global_costmap.info.origin.position.z = 0.0;
+ srv.request.global_costmap.info.origin.orientation.w = 1.0;
+
+ srv.request.global_costmap.data.resize(
+ srv.request.global_costmap.info.width *
+ srv.request.global_costmap.info.height);
+
+ unsigned char* data = costmap_ros_->getCostmap()->getCharMap();
+ for (unsigned int i = 0; i < srv.request.global_costmap.data.size(); i++) {
+ srv.request.global_costmap.data[i] = cost_translation_table_[ data[ i ]];
+ }
+ initialize_client_.call(srv);
+ delete cost_translation_table_;
+ }
+
+ 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_) {
+ omg_ros_nav_bridge::ComputeVelCmd srv;
+ srv.request.position.x = current_pose_.getOrigin().getX();
+ srv.request.position.y = current_pose_.getOrigin().getY();
+ srv.request.position.theta = tf::getYaw(current_pose_.getRotation());
+
+ tf::Stamped robot_vel;
+ odom_helper_.getRobotVel(robot_vel);
+ srv.request.vel_in.linear.x = robot_vel.getOrigin().getX();
+ srv.request.vel_in.linear.y = robot_vel.getOrigin().getY();
+ srv.request.vel_in.angular.z = tf::getYaw(robot_vel.getRotation());
+
+ compute_velocity_client_.call(srv);
+
+ if (!srv.response.computed) {
+ ROS_ERROR("Could not calculate the velocity");
+ return false;
+ }
+ local_plan_.clear();
+ local_plan_.resize(srv.response.x_traj.size());
+ for (size_t i = 0; i < srv.response.x_traj.size(); ++i) {
+ geometry_msgs::PoseStamped p;
+ p.header.stamp = ros::Time::now();
+ p.header.frame_id = "odom";
+ p.pose.position.x = srv.response.x_traj[i];
+ p.pose.position.y = srv.response.y_traj[i];
+ local_plan_.push_back(std::move(p));
+ }
+ publishLocalPlan(local_plan_);
+ cmd_vel = srv.response.cmd_vel;
+ } else {
+ ROS_ERROR_STREAM("compute_velocity_client_ is not connected.");
+ return false;
+ }
+ return true;
+}
+
+bool OMGPlannerROS::setPlan(
+ const std::vector &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_) {
+ omg_ros_nav_bridge::ConfigPlanner srv;
+ srv.request.waypoint_lst.reserve(orig_global_plan.size());
+ for (geometry_msgs::PoseStamped p : orig_global_plan) {
+ geometry_msgs::Pose2D pose;
+ pose.x = p.pose.position.x;
+ pose.y = p.pose.position.y;
+ pose.theta = tf::getYaw(p.pose.orientation);
+ srv.request.waypoint_lst.push_back(pose);
+ }
+ set_plan_client_.call(srv);
+ if (!srv.response.planned) {
+ ROS_ERROR("Could not generate OMG plan!");
+ return false;
+ }
+ } else {
+ ROS_ERROR_STREAM("set_plan_client_ is not connected.");
+ return false;
+ }
+ publishGlobalPlan(orig_global_plan);
+ return true;
+}
+
+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(
+ const std::vector &path) {
+ base_local_planner::publishPlan(path, l_plan_pub_);
+}
+
+void OMGPlannerROS::publishGlobalPlan(const std::vector &path) {
+ base_local_planner::publishPlan(path, g_plan_pub_);
+}
+}