From 0cd84dfbf7ae5b1f06597693e896eeb919489485 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Mon, 5 Mar 2018 13:37:04 +0100 Subject: [PATCH 01/13] Initial commit of the omg-local-planner --- omg_local_planner/CMakeLists.txt | 47 ++++++++ omg_local_planner/blp_plugin.xml | 7 ++ .../include/omg_local_planner/omg_planner.h | 12 ++ .../omg_local_planner/omg_planner_ros.h | 99 +++++++++++++++++ omg_local_planner/package.xml | 31 ++++++ omg_local_planner/src/omg_planner.cpp | 0 omg_local_planner/src/omg_planner_ros.cpp | 105 ++++++++++++++++++ 7 files changed, 301 insertions(+) create mode 100644 omg_local_planner/CMakeLists.txt create mode 100644 omg_local_planner/blp_plugin.xml create mode 100644 omg_local_planner/include/omg_local_planner/omg_planner.h create mode 100644 omg_local_planner/include/omg_local_planner/omg_planner_ros.h create mode 100644 omg_local_planner/package.xml create mode 100644 omg_local_planner/src/omg_planner.cpp create mode 100644 omg_local_planner/src/omg_planner_ros.cpp 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..39a629cec2 --- /dev/null +++ b/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 +#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(std::vector& path); + + /** + * @brief Publish the global plan to be followed. + * @param path A set of poses composing the plan. + */ + void publishGlobalPlan(std::vector& 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 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 diff --git a/omg_local_planner/package.xml b/omg_local_planner/package.xml new file mode 100644 index 0000000000..5aa2ac93fe --- /dev/null +++ b/omg_local_planner/package.xml @@ -0,0 +1,31 @@ + + + omg_local_planner + 0.0.0 + The omg_local_planner package + + niko + + + 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..0a2852b05d --- /dev/null +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -0,0 +1,105 @@ +#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::__cxx11::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(goal_reached_srv_, -1); + goal_reached_client_ = + public_nh.serviceClient( + 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 &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 &path) { + base_local_planner::publishPlan(path, l_plan_pub_); +} + +void OMGPlannerROS::publishGlobalPlan( + std::vector &path) { + base_local_planner::publishPlan(path, g_plan_pub_); +} +} From 0522916659f359e9182eef25ecc0fbc8abe19df0 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Tue, 6 Mar 2018 17:44:53 +0100 Subject: [PATCH 02/13] Change the maintainer --- omg_local_planner/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omg_local_planner/package.xml b/omg_local_planner/package.xml index 5aa2ac93fe..f7e3ba50e4 100644 --- a/omg_local_planner/package.xml +++ b/omg_local_planner/package.xml @@ -4,7 +4,7 @@ 0.0.0 The omg_local_planner package - niko + Intermodalics BVBA Closed From e1a2a6474b348edfc9aa674826c52401afa52e86 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Tue, 6 Mar 2018 17:45:38 +0100 Subject: [PATCH 03/13] Document variables --- .../omg_local_planner/omg_planner_ros.h | 63 +++++++++++++++---- 1 file changed, 52 insertions(+), 11 deletions(-) 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 index 39a629cec2..f4fb6d8545 100644 --- a/omg_local_planner/include/omg_local_planner/omg_planner_ros.h +++ b/omg_local_planner/include/omg_local_planner/omg_planner_ros.h @@ -79,20 +79,61 @@ class OMGPlannerROS : public nav_core::BaseLocalPlanner { */ void publishGlobalPlan(std::vector& path); - bool initialized_; + 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_; - tf::Stamped 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"; + 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 goal_reached_srv_ = "goal_reached"; + + /** + * @brief goal_reached_srv_ Service name. + */ + const std::string set_plan_srv_ = "set_plan"; + /** + * @brief goal_reached_srv_ Service name. + */ + const std::string initialize_srv_ = "initialize"; + + /** + * @brief goal_reached_srv_ Service name. + */ + const std::string compute_velocity_srv_ = "compute_velocity"; }; } From 8e6a30a2a0ce581ed8200d1d01f810bee39da851 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Thu, 8 Mar 2018 11:47:37 +0100 Subject: [PATCH 04/13] Rename constants to standard --- .../include/omg_local_planner/omg_planner_ros.h | 8 ++++---- omg_local_planner/src/omg_planner_ros.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) 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 index f4fb6d8545..e31296edf6 100644 --- a/omg_local_planner/include/omg_local_planner/omg_planner_ros.h +++ b/omg_local_planner/include/omg_local_planner/omg_planner_ros.h @@ -118,22 +118,22 @@ class OMGPlannerROS : public nav_core::BaseLocalPlanner { /** * @brief goal_reached_srv_ Service name. */ - const std::string goal_reached_srv_ = "goal_reached"; + const std::string kGoalReachedSrv_ = "goal_reached"; /** * @brief goal_reached_srv_ Service name. */ - const std::string set_plan_srv_ = "set_plan"; + const std::string kSetPlanSrv_ = "set_plan"; /** * @brief goal_reached_srv_ Service name. */ - const std::string initialize_srv_ = "initialize"; + const std::string kInitializeSrv_ = "initialize"; /** * @brief goal_reached_srv_ Service name. */ - const std::string compute_velocity_srv_ = "compute_velocity"; + const std::string kComputeVelocitySrv_ = "compute_velocity"; }; } diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp index 0a2852b05d..630824e5b6 100644 --- a/omg_local_planner/src/omg_planner_ros.cpp +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -24,10 +24,10 @@ void OMGPlannerROS::initialize(std::__cxx11::string name, // Initialise the service clients. ros::NodeHandle public_nh; - ros::service::waitForService(goal_reached_srv_, -1); + ros::service::waitForService(kGoalReachedSrv_, -1); goal_reached_client_ = public_nh.serviceClient( - goal_reached_srv_, true); + kGoalReachedSrv_, true); // TODO: Add the other services and call initialise initialized_ = true; } From 5ad7a781190d81d2c8da70e1cecd4cdbf9436023 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Thu, 22 Mar 2018 17:13:32 +0100 Subject: [PATCH 05/13] Add extra service headers --- omg_local_planner/src/omg_planner_ros.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp index 630824e5b6..0b43623e8c 100644 --- a/omg_local_planner/src/omg_planner_ros.cpp +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -2,7 +2,10 @@ #include +#include +#include #include +#include // register this planner as a BaseLocalPlanner plugin PLUGINLIB_EXPORT_CLASS(omg_local_planner::OMGPlannerROS, From dc9a9daf1d3530f56459691b9f15da5a7839b0a5 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Tue, 27 Mar 2018 17:22:24 +0200 Subject: [PATCH 06/13] Finalise service calls to OMG --- .../omg_local_planner/omg_planner_ros.h | 6 +- omg_local_planner/src/omg_planner_ros.cpp | 57 ++++++++++++++++++- 2 files changed, 57 insertions(+), 6 deletions(-) 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 index e31296edf6..002415eab9 100644 --- a/omg_local_planner/include/omg_local_planner/omg_planner_ros.h +++ b/omg_local_planner/include/omg_local_planner/omg_planner_ros.h @@ -123,17 +123,17 @@ class OMGPlannerROS : public nav_core::BaseLocalPlanner { /** * @brief goal_reached_srv_ Service name. */ - const std::string kSetPlanSrv_ = "set_plan"; + const std::string kSetPlanSrv_ = "config_planner"; /** * @brief goal_reached_srv_ Service name. */ - const std::string kInitializeSrv_ = "initialize"; + const std::string kInitializeSrv_ = "init_planner"; /** * @brief goal_reached_srv_ Service name. */ - const std::string kComputeVelocitySrv_ = "compute_velocity"; + const std::string kComputeVelocitySrv_ = "compute_vel_cmd"; }; } diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp index 0b43623e8c..36e12a2bc5 100644 --- a/omg_local_planner/src/omg_planner_ros.cpp +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -31,7 +31,24 @@ void OMGPlannerROS::initialize(std::__cxx11::string name, goal_reached_client_ = public_nh.serviceClient( kGoalReachedSrv_, true); - // TODO: Add the other services and call initialise + 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; + srv.request.width = 10.0; + srv.request.height = 5.0; + omg_ros_nav_bridge::static_obst obs; + obs.single_obstacle = {-2.0, -2.3, 1.5, 4.0, 0.1}; + srv.request.obstacles.push_back(obs); + initialize_client_.call(srv); + initialized_ = true; } } @@ -45,10 +62,29 @@ bool OMGPlannerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) { } if (compute_velocity_client_) { - // TODO: Add call to service + 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; + } + cmd_vel = srv.response.cmd_vel; } else { ROS_ERROR_STREAM("compute_velocity_client_ is not connected."); + return false; } + return true; } bool OMGPlannerROS::setPlan( @@ -62,10 +98,25 @@ bool OMGPlannerROS::setPlan( ROS_INFO("Got new plan"); if (set_plan_client_) { - // TODO: Add call to service + 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; } + return true; } bool OMGPlannerROS::isGoalReached() { From eb237f4a91d02400aaeb9731ecfd67ba9dc05f8a Mon Sep 17 00:00:00 2001 From: Nick Tsiogkas Date: Wed, 28 Mar 2018 17:22:52 +0200 Subject: [PATCH 07/13] Remove cpp11 namespace resolution --- omg_local_planner/src/omg_planner_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp index 36e12a2bc5..602cc19e01 100644 --- a/omg_local_planner/src/omg_planner_ros.cpp +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -14,7 +14,7 @@ PLUGINLIB_EXPORT_CLASS(omg_local_planner::OMGPlannerROS, namespace omg_local_planner { OMGPlannerROS::OMGPlannerROS() : initialized_(false), odom_helper_("odom") {} -void OMGPlannerROS::initialize(std::__cxx11::string name, +void OMGPlannerROS::initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros) { if (!initialized_) { From 1ec63be5898284241bf8d72022624e268e64180c Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Fri, 6 Apr 2018 14:49:29 +0200 Subject: [PATCH 08/13] Start publishing the costmap in the init service --- omg_local_planner/src/omg_planner_ros.cpp | 56 ++++++++++++++++++++--- 1 file changed, 50 insertions(+), 6 deletions(-) diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp index 602cc19e01..56b6eada7e 100644 --- a/omg_local_planner/src/omg_planner_ros.cpp +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -42,12 +42,56 @@ void OMGPlannerROS::initialize(std::string name, kComputeVelocitySrv_, true); omg_ros_nav_bridge::InitPlanner srv; - srv.request.width = 10.0; - srv.request.height = 5.0; - omg_ros_nav_bridge::static_obst obs; - obs.single_obstacle = {-2.0, -2.3, 1.5, 4.0, 0.1}; - srv.request.obstacles.push_back(obs); - initialize_client_.call(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; } From e6d5bfa4c6b4ea15e2f264ac9b6bf29c0d269178 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Mon, 30 Apr 2018 12:02:08 +0200 Subject: [PATCH 09/13] Add plan visualisation --- .../include/omg_local_planner/omg_planner_ros.h | 9 +++++++-- omg_local_planner/src/omg_planner_ros.cpp | 14 +++++++++++--- 2 files changed, 18 insertions(+), 5 deletions(-) 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 index 002415eab9..080891675f 100644 --- a/omg_local_planner/include/omg_local_planner/omg_planner_ros.h +++ b/omg_local_planner/include/omg_local_planner/omg_planner_ros.h @@ -71,13 +71,13 @@ class OMGPlannerROS : public nav_core::BaseLocalPlanner { * @brief Publish the local plan to be followed. * @param path A set of poses composing the plan. */ - void publishLocalPlan(std::vector& path); + 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(std::vector& path); + void publishGlobalPlan(const std::vector& path); bool initialized_; ///< @brief Holds the planner initialization status. @@ -134,6 +134,11 @@ class OMGPlannerROS : public nav_core::BaseLocalPlanner { * @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_; }; } diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp index 56b6eada7e..21dc0e391e 100644 --- a/omg_local_planner/src/omg_planner_ros.cpp +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -123,6 +123,14 @@ bool OMGPlannerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) { 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.pose.position.x = srv.response.x_traj[i]; + p.pose.position.y = srv.response.y_traj[i]; + local_plan_.push_back(std::move(p)); + } cmd_vel = srv.response.cmd_vel; } else { ROS_ERROR_STREAM("compute_velocity_client_ is not connected."); @@ -160,6 +168,7 @@ bool OMGPlannerROS::setPlan( ROS_ERROR_STREAM("set_plan_client_ is not connected."); return false; } + publishGlobalPlan(orig_global_plan); return true; } @@ -192,12 +201,11 @@ bool OMGPlannerROS::isGoalReached() { bool OMGPlannerROS::isInitialized() { return initialized_; } void OMGPlannerROS::publishLocalPlan( - std::vector &path) { + const std::vector &path) { base_local_planner::publishPlan(path, l_plan_pub_); } -void OMGPlannerROS::publishGlobalPlan( - std::vector &path) { +void OMGPlannerROS::publishGlobalPlan(const std::vector &path) { base_local_planner::publishPlan(path, g_plan_pub_); } } From 0db8c4d4186679054762a277e1e17467e1f6cc32 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Tue, 1 May 2018 15:16:41 +0200 Subject: [PATCH 10/13] Add header frame and call local plan publisher --- omg_local_planner/src/omg_planner_ros.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp index 21dc0e391e..7c40f88dd5 100644 --- a/omg_local_planner/src/omg_planner_ros.cpp +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -127,10 +127,12 @@ bool OMGPlannerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) { 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.frame_id = "/map"; 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."); From 47850d9ff1ed2ae221d77a99c4061463a22f6bb4 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Tue, 1 May 2018 16:34:24 +0200 Subject: [PATCH 11/13] Change to odom frame as the global path and add stamps --- omg_local_planner/src/omg_planner_ros.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/omg_local_planner/src/omg_planner_ros.cpp b/omg_local_planner/src/omg_planner_ros.cpp index 7c40f88dd5..30b14c47a5 100644 --- a/omg_local_planner/src/omg_planner_ros.cpp +++ b/omg_local_planner/src/omg_planner_ros.cpp @@ -127,7 +127,8 @@ bool OMGPlannerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) { 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.frame_id = "/map"; + 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)); From 1fc18f8f50d039adde25cc217a65e3874a893c50 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Mon, 25 Mar 2019 13:28:26 +0100 Subject: [PATCH 12/13] Add local planner boilerplate --- gip_local_planner/CMakeLists.txt | 46 ++++++++ gip_local_planner/glp_plugin.xml | 7 ++ .../gip_local_planner/gip_planner_ros.h | 104 ++++++++++++++++++ gip_local_planner/package.xml | 30 +++++ gip_local_planner/src/gip_planner_ros.cpp | 55 +++++++++ 5 files changed, 242 insertions(+) create mode 100644 gip_local_planner/CMakeLists.txt create mode 100644 gip_local_planner/glp_plugin.xml create mode 100644 gip_local_planner/include/gip_local_planner/gip_planner_ros.h create mode 100644 gip_local_planner/package.xml create mode 100644 gip_local_planner/src/gip_planner_ros.cpp diff --git a/gip_local_planner/CMakeLists.txt b/gip_local_planner/CMakeLists.txt new file mode 100644 index 0000000000..ca2cb2ff90 --- /dev/null +++ b/gip_local_planner/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gip_local_planner) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + base_local_planner + costmap_2d + geometry_msgs + nav_core + nav_msgs + pluginlib + roscpp + std_msgs + tf + rosconsole +) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES omg_local_planner + CATKIN_DEPENDS geometry_msgs nav_msgs pluginlib roscpp +) + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} src/gip_planner_ros.cpp) +add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) + +install(FILES glp_plugin.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + PATTERN ".svn" EXCLUDE +) diff --git a/gip_local_planner/glp_plugin.xml b/gip_local_planner/glp_plugin.xml new file mode 100644 index 0000000000..e001e33fa9 --- /dev/null +++ b/gip_local_planner/glp_plugin.xml @@ -0,0 +1,7 @@ + + + + Implementation of a local planner. + + + diff --git a/gip_local_planner/include/gip_local_planner/gip_planner_ros.h b/gip_local_planner/include/gip_local_planner/gip_planner_ros.h new file mode 100644 index 0000000000..da959e6a5d --- /dev/null +++ b/gip_local_planner/include/gip_local_planner/gip_planner_ros.h @@ -0,0 +1,104 @@ +#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 gip_local_planner { +/** + * @class GIPPlannerROS + * @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 GIPPlannerROS : public nav_core::BaseLocalPlanner { + public: + /** + * @brief Constructor for the GIPPlannerROS wrapper. + */ + GIPPlannerROS(); + + /** + * @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. + */ + ~GIPPlannerROS(); + + /** + * @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 local_plan_ Store local plan for visualisation. + */ + std::vector local_plan_; +}; +} + +#endif // OMG_LOCAL_PLANNER_OMG_PLANNER_ROS_H diff --git a/gip_local_planner/package.xml b/gip_local_planner/package.xml new file mode 100644 index 0000000000..26aa784918 --- /dev/null +++ b/gip_local_planner/package.xml @@ -0,0 +1,30 @@ + + + gip_local_planner + 0.0.0 + The gip_local_planner package + + Intermodalics BVBA + + + Closed + + + Nikolaos Tsiogkas + + catkin + base_local_planner + costmap_2d + geometry_msgs + nav_core + nav_msgs + pluginlib + roscpp + std_msgs + tf + rosconsole + + + + + diff --git a/gip_local_planner/src/gip_planner_ros.cpp b/gip_local_planner/src/gip_planner_ros.cpp new file mode 100644 index 0000000000..e882d5599a --- /dev/null +++ b/gip_local_planner/src/gip_planner_ros.cpp @@ -0,0 +1,55 @@ +#include + +#include + +// register this planner as a BaseLocalPlanner plugin +PLUGINLIB_EXPORT_CLASS(gip_local_planner::GIPPlannerROS, + nav_core::BaseLocalPlanner) + +namespace gip_local_planner { +GIPPlannerROS::GIPPlannerROS() : initialized_(false), odom_helper_("odom") {} + +void GIPPlannerROS::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_); + initialized_ = true; + } +} + +GIPPlannerROS::~GIPPlannerROS() {} + +bool GIPPlannerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) { + if (!costmap_ros_->getRobotPose(current_pose_)) { + ROS_ERROR("Could not get robot pose"); + return false; + } + return false; +} + +bool GIPPlannerROS::setPlan( + const std::vector &orig_global_plan) { +return false; +} + +bool GIPPlannerROS::isGoalReached() { + return false; +} + +bool GIPPlannerROS::isInitialized() { return initialized_; } + +void GIPPlannerROS::publishLocalPlan( + const std::vector &path) { + base_local_planner::publishPlan(path, l_plan_pub_); +} + +void GIPPlannerROS::publishGlobalPlan(const std::vector &path) { + base_local_planner::publishPlan(path, g_plan_pub_); +} +} From 05a807be983d45a269e55b7b49475be1cd4a6f35 Mon Sep 17 00:00:00 2001 From: Nikolaos Tsiogkas Date: Fri, 5 Apr 2019 10:29:19 +0200 Subject: [PATCH 13/13] Revert "Add local planner boilerplate" This reverts commit 1fc18f8f50d039adde25cc217a65e3874a893c50. --- gip_local_planner/CMakeLists.txt | 46 -------- gip_local_planner/glp_plugin.xml | 7 -- .../gip_local_planner/gip_planner_ros.h | 104 ------------------ gip_local_planner/package.xml | 30 ----- gip_local_planner/src/gip_planner_ros.cpp | 55 --------- 5 files changed, 242 deletions(-) delete mode 100644 gip_local_planner/CMakeLists.txt delete mode 100644 gip_local_planner/glp_plugin.xml delete mode 100644 gip_local_planner/include/gip_local_planner/gip_planner_ros.h delete mode 100644 gip_local_planner/package.xml delete mode 100644 gip_local_planner/src/gip_planner_ros.cpp diff --git a/gip_local_planner/CMakeLists.txt b/gip_local_planner/CMakeLists.txt deleted file mode 100644 index ca2cb2ff90..0000000000 --- a/gip_local_planner/CMakeLists.txt +++ /dev/null @@ -1,46 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(gip_local_planner) - -add_compile_options(-std=c++11) - -find_package(catkin REQUIRED COMPONENTS - base_local_planner - costmap_2d - geometry_msgs - nav_core - nav_msgs - pluginlib - roscpp - std_msgs - tf - rosconsole -) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES omg_local_planner - CATKIN_DEPENDS geometry_msgs nav_msgs pluginlib roscpp -) - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -add_library(${PROJECT_NAME} src/gip_planner_ros.cpp) -add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - ) - -install(FILES glp_plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - PATTERN ".svn" EXCLUDE -) diff --git a/gip_local_planner/glp_plugin.xml b/gip_local_planner/glp_plugin.xml deleted file mode 100644 index e001e33fa9..0000000000 --- a/gip_local_planner/glp_plugin.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Implementation of a local planner. - - - diff --git a/gip_local_planner/include/gip_local_planner/gip_planner_ros.h b/gip_local_planner/include/gip_local_planner/gip_planner_ros.h deleted file mode 100644 index da959e6a5d..0000000000 --- a/gip_local_planner/include/gip_local_planner/gip_planner_ros.h +++ /dev/null @@ -1,104 +0,0 @@ -#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 gip_local_planner { -/** - * @class GIPPlannerROS - * @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 GIPPlannerROS : public nav_core::BaseLocalPlanner { - public: - /** - * @brief Constructor for the GIPPlannerROS wrapper. - */ - GIPPlannerROS(); - - /** - * @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. - */ - ~GIPPlannerROS(); - - /** - * @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 local_plan_ Store local plan for visualisation. - */ - std::vector local_plan_; -}; -} - -#endif // OMG_LOCAL_PLANNER_OMG_PLANNER_ROS_H diff --git a/gip_local_planner/package.xml b/gip_local_planner/package.xml deleted file mode 100644 index 26aa784918..0000000000 --- a/gip_local_planner/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - gip_local_planner - 0.0.0 - The gip_local_planner package - - Intermodalics BVBA - - - Closed - - - Nikolaos Tsiogkas - - catkin - base_local_planner - costmap_2d - geometry_msgs - nav_core - nav_msgs - pluginlib - roscpp - std_msgs - tf - rosconsole - - - - - diff --git a/gip_local_planner/src/gip_planner_ros.cpp b/gip_local_planner/src/gip_planner_ros.cpp deleted file mode 100644 index e882d5599a..0000000000 --- a/gip_local_planner/src/gip_planner_ros.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include - -#include - -// register this planner as a BaseLocalPlanner plugin -PLUGINLIB_EXPORT_CLASS(gip_local_planner::GIPPlannerROS, - nav_core::BaseLocalPlanner) - -namespace gip_local_planner { -GIPPlannerROS::GIPPlannerROS() : initialized_(false), odom_helper_("odom") {} - -void GIPPlannerROS::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_); - initialized_ = true; - } -} - -GIPPlannerROS::~GIPPlannerROS() {} - -bool GIPPlannerROS::computeVelocityCommands(geometry_msgs::Twist &cmd_vel) { - if (!costmap_ros_->getRobotPose(current_pose_)) { - ROS_ERROR("Could not get robot pose"); - return false; - } - return false; -} - -bool GIPPlannerROS::setPlan( - const std::vector &orig_global_plan) { -return false; -} - -bool GIPPlannerROS::isGoalReached() { - return false; -} - -bool GIPPlannerROS::isInitialized() { return initialized_; } - -void GIPPlannerROS::publishLocalPlan( - const std::vector &path) { - base_local_planner::publishPlan(path, l_plan_pub_); -} - -void GIPPlannerROS::publishGlobalPlan(const std::vector &path) { - base_local_planner::publishPlan(path, g_plan_pub_); -} -}