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
00036
00037 #include "collvoid_simple_global_planner/collvoid_simple_global_planner.h"
00038 #include <pluginlib/class_list_macros.h>
00039
00040
00041
00042 PLUGINLIB_DECLARE_CLASS(collvoid_simple_global_planner, CollvoidSimpleGlobalPlanner, collvoid_simple_global_planner::CollvoidSimpleGlobalPlanner, nav_core::BaseGlobalPlanner)
00043
00044 namespace collvoid_simple_global_planner {
00045
00046 CollvoidSimpleGlobalPlanner::CollvoidSimpleGlobalPlanner()
00047 {}
00048
00049 CollvoidSimpleGlobalPlanner::CollvoidSimpleGlobalPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00050 {
00051 initialize(name, costmap_ros);
00052 }
00053
00054 void CollvoidSimpleGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros){
00055 }
00056
00057 bool CollvoidSimpleGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start,
00058 const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){
00059
00060 ROS_DEBUG("Got a start: %.2f, %.2f, and a goal: %.2f, %.2f", start.pose.position.x, start.pose.position.y, goal.pose.position.x, goal.pose.position.y);
00061
00062 plan.clear();
00063 plan.push_back(start);
00064 double x, y, dir_x, dir_y;
00065 dir_x = goal.pose.position.x - start.pose.position.x;
00066 dir_y = goal.pose.position.y - start.pose.position.y;
00067 double length = sqrt(dir_y * dir_y + dir_x * dir_x);
00068 dir_x /= length;
00069 dir_y /= length;
00070 x = start.pose.position.x + 0.1 * dir_x;
00071 y = start.pose.position.y + 0.1 * dir_y;
00072 ROS_DEBUG("dir: %.2f, %.2f, cur: %.2f, %.2f", dir_x, dir_y, x, y);
00073
00074 while (fabs(x-goal.pose.position.x) > 0.2 || fabs(y-goal.pose.position.y) > 0.2) {
00075 geometry_msgs::PoseStamped point;
00076 point.header = goal.header;
00077 point.pose.position.x = x;
00078 point.pose.position.y = y;
00079 point.pose.orientation.w = 1;
00080 plan.push_back(point);
00081 x += 0.1 * dir_x;
00082 y += 0.1 * dir_y;
00083 }
00084 plan.push_back(goal);
00085
00086 return true;
00087 }
00088
00089 };