Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <nav_core_adapter/local_planner_adapter.h>
00036 #include <nav_core_adapter/costmap_adapter.h>
00037 #include <nav_core_adapter/shared_pointers.h>
00038 #include <nav_2d_utils/conversions.h>
00039 #include <nav_2d_utils/tf_help.h>
00040 #include <nav_core2/exceptions.h>
00041 #include <pluginlib/class_list_macros.h>
00042 #include <string>
00043 #include <vector>
00044
00045 namespace nav_core_adapter
00046 {
00047
00048 LocalPlannerAdapter::LocalPlannerAdapter() :
00049 planner_loader_("nav_core2", "nav_core2::LocalPlanner")
00050 {
00051 }
00052
00056 void LocalPlannerAdapter::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros)
00057 {
00058 tf_ = createSharedPointerWithNoDelete<tf::TransformListener>(tf);
00059 costmap_ros_ = costmap_ros;
00060 costmap_adapter_ = std::make_shared<CostmapAdapter>();
00061 costmap_adapter_->initialize(costmap_ros);
00062
00063 ros::NodeHandle nh;
00064 ros::NodeHandle private_nh("~");
00065 ros::NodeHandle adapter_nh("~/" + name);
00066 std::string planner_name;
00067 adapter_nh.param("planner_name", planner_name, std::string("dwb_local_planner::DWBLocalPlanner"));
00068 ROS_INFO_NAMED("LocalPlannerAdapter", "Loading plugin %s", planner_name.c_str());
00069 planner_ = planner_loader_.createInstance(planner_name);
00070 planner_->initialize(private_nh, planner_loader_.getName(planner_name), tf_, costmap_adapter_);
00071 has_active_goal_ = false;
00072
00073 odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh);
00074 }
00075
00079 bool LocalPlannerAdapter::computeVelocityCommands(geometry_msgs::Twist& cmd_vel)
00080 {
00081 if (!has_active_goal_)
00082 {
00083 return false;
00084 }
00085
00086
00087 nav_2d_msgs::Pose2DStamped pose2d;
00088 if (!getRobotPose(pose2d))
00089 {
00090 return false;
00091 }
00092
00093
00094 nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
00095
00096 nav_2d_msgs::Twist2DStamped cmd_vel_2d;
00097 try
00098 {
00099 cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
00100 }
00101 catch (const nav_core2::PlannerException& e)
00102 {
00103 ROS_ERROR_NAMED("LocalPlannerAdapter", "computeVelocityCommands exception: %s", e.what());
00104 return false;
00105 }
00106 cmd_vel = nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
00107 return true;
00108 }
00109
00113 bool LocalPlannerAdapter::isGoalReached()
00114 {
00115
00116 nav_2d_msgs::Pose2DStamped pose2d;
00117 if (!getRobotPose(pose2d))
00118 return false;
00119
00120 nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
00121 bool ret = planner_->isGoalReached(pose2d, velocity);
00122 if (ret)
00123 {
00124 has_active_goal_ = false;
00125 }
00126 return ret;
00127 }
00128
00132 bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)
00133 {
00134 nav_2d_msgs::Path2D path = nav_2d_utils::posesToPath2D(orig_global_plan);
00135 try
00136 {
00137 if (path.poses.size() > 0)
00138 {
00139 nav_2d_msgs::Pose2DStamped goal_pose;
00140 goal_pose.header = path.header;
00141 goal_pose.pose = path.poses.back();
00142
00143 if (!has_active_goal_ || hasGoalChanged(goal_pose))
00144 {
00145 last_goal_ = goal_pose;
00146 has_active_goal_ = true;
00147 planner_->setGoalPose(goal_pose);
00148 }
00149 }
00150
00151 planner_->setPlan(path);
00152 return true;
00153 }
00154 catch (const nav_core2::PlannerException& e)
00155 {
00156 ROS_ERROR_NAMED("LocalPlannerAdapter", "setPlan Exception: %s", e.what());
00157 return false;
00158 }
00159 }
00160
00161 bool LocalPlannerAdapter::hasGoalChanged(const nav_2d_msgs::Pose2DStamped& new_goal)
00162 {
00163 if (last_goal_.header.frame_id != new_goal.header.frame_id)
00164 {
00165 return true;
00166 }
00167
00168 return last_goal_.pose.x != new_goal.pose.x || last_goal_.pose.y != new_goal.pose.y ||
00169 last_goal_.pose.theta != new_goal.pose.theta;
00170 }
00171
00172 bool LocalPlannerAdapter::getRobotPose(nav_2d_msgs::Pose2DStamped& pose2d)
00173 {
00174 tf::Stamped<tf::Pose> current_pose;
00175 if (!costmap_ros_->getRobotPose(current_pose))
00176 {
00177 ROS_ERROR_NAMED("LocalPlannerAdapter", "Could not get robot pose");
00178 return false;
00179 }
00180 pose2d = nav_2d_utils::stampedPoseToPose2D(current_pose);
00181 return true;
00182 }
00183
00184 }
00185
00186
00187 PLUGINLIB_EXPORT_CLASS(nav_core_adapter::LocalPlannerAdapter, nav_core::BaseLocalPlanner)