50 planner_loader_(
"nav_core2",
"nav_core2::LocalPlanner")
59 tf_ = createSharedPointerWithNoDelete<tf::TransformListener>(tf);
67 std::string planner_name;
68 adapter_nh.
param(
"planner_name", planner_name, std::string(
"dwb_local_planner::DWBLocalPlanner"));
69 ROS_INFO_NAMED(
"LocalPlannerAdapter",
"Loading plugin %s", planner_name.c_str());
74 odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh);
88 nav_2d_msgs::Pose2DStamped pose2d;
95 nav_2d_msgs::Twist2D velocity =
odom_sub_->getTwist();
97 nav_2d_msgs::Twist2DStamped cmd_vel_2d;
100 cmd_vel_2d =
planner_->computeVelocityCommands(pose2d, velocity);
104 ROS_ERROR_NAMED(
"LocalPlannerAdapter",
"computeVelocityCommands exception: %s", e.what());
117 nav_2d_msgs::Pose2DStamped pose2d;
121 nav_2d_msgs::Twist2D velocity =
odom_sub_->getTwist();
122 bool ret =
planner_->isGoalReached(pose2d, velocity);
138 if (path.poses.size() > 0)
140 nav_2d_msgs::Pose2DStamped goal_pose;
141 goal_pose.header = path.header;
142 goal_pose.pose = path.poses.back();
157 ROS_ERROR_NAMED(
"LocalPlannerAdapter",
"setPlan Exception: %s", e.what());
164 if (
last_goal_.header.frame_id != new_goal.header.frame_id)
bool hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal)
See if the back of the global plan matches the most recent goal pose.
#define ROS_INFO_NAMED(name,...)
std::shared_ptr< CostmapAdapter > costmap_adapter_
costmap_2d::Costmap2DROS * costmap_ros_
bool getRobotPose(tf::Stamped< tf::Pose > &global_pose) const
std::shared_ptr< nav_2d_utils::OdomSubscriber > odom_sub_
helper class for subscribing to odometry
bool isGoalReached() override
Collect the additional information needed by nav_core2 and call the child isGoalReached.
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) override
Collect the additional information needed by nav_core2 and call the child computeVelocityCommands.
geometry_msgs::Twist twist2Dto3D(const nav_2d_msgs::Twist2D &cmd_vel_2d)
nav_2d_msgs::Path2D posesToPath2D(const std::vector< geometry_msgs::PoseStamped > &poses)
nav_2d_msgs::Pose2DStamped last_goal_
bool getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d)
Get the robot pose from the costmap and store as Pose2DStamped.
used for employing a nav_core2 local planner (such as dwb) as a nav_core plugin, like in move_base...
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
virtual std::string getName(const std::string &lookup_name)
nav_2d_msgs::Pose2DStamped stampedPoseToPose2D(const tf::Stamped< tf::Pose > &pose)
bool setPlan(const std::vector< geometry_msgs::PoseStamped > &plan) override
Convert from 2d to 3d and call child setPlan.
#define ROS_ERROR_NAMED(name,...)
void initialize(std::string name, tf::TransformListener *tf, costmap_2d::Costmap2DROS *costmap_ros) override
Load the nav_core2 local planner and initialize it.
boost::shared_ptr< nav_core2::LocalPlanner > planner_
pluginlib::ClassLoader< nav_core2::LocalPlanner > planner_loader_