Bhaskara, This is different in that it will actually create plans to space that's completely occupied where as the default_tolerance parameter guarantees that the endpoint of the plan is in free space. It just searches in a window around the user-specified goal After thinking about it a little though, maybe clearing obstacles out in this situation isn't such a good thing, and it does seem like the use case Christian has would have been solved by setting the default_tolerance parameter. Christian, is there any reason that you want to actually clear the obstacles from the goal region as opposed to attempting to find a valid plan within a wider region without clearing obstacles? If there's no compelling reason for it, perhaps I'll back out the change. Hope all is well, Eitan On Wed, Dec 1, 2010 at 10:33 AM, Bhaskara Marthi wrote: > > > On Mon, Nov 29, 2010 at 9:43 AM, Eitan Marder-Eppstein < > eitan@willowgarage.com> wrote: > >> Seems like a reasonable feature to expose. Applied the patch in r34304 in >> navigation trunk. Should go into D-Turtle. >> >> Thanks for passing it on, >> >> Eitan >> >> >> Couldn't we get this behavior by just setting the default_tolerance > parameter of navfn? > - Bhaskara > > >> On Fri, Nov 26, 2010 at 3:35 AM, Christian Verbeek < >> verbeek@servicerobotics.eu> wrote: >> >>> Dear Eitan, >>> >>> I found it useful to enable navfn to be able to create a plan even if >>> the goal position hits an obstacle. This is useful when you set goal >>> positions through a user interface. If the user wants the robot to move >>> near to walls or other obstacles then it is very likely that the goal >>> position hits the obstacle and the robot does not move at all. Instead >>> the intended behaviour is to get as close as possible and then stop if >>> the robots sensors detect an obstacle. >>> >>> I modified nafvn_ros to clear a region in the costmap around the goal >>> position before making the plan. This behaviour must be switched on by >>> setting the clear_goal_position parameter to true (default is false >>> keeping everything as before). >>> >>> What do you think? Could this be integrated into the ros mainstream? >>> Would make things easier for me. >>> >>> The patch is generated from stacks/navigation/navfn >>> >>> Best wishes >>> Christian >>> >>> Index: include/navfn/navfn_ros.h >>> =================================================================== >>> --- include/navfn/navfn_ros.h (revision 34269) >>> +++ include/navfn/navfn_ros.h (working copy) >>> @@ -132,7 +132,7 @@ >>> boost::shared_ptr planner_; >>> double inscribed_radius_, circumscribed_radius_, inflation_radius_; >>> ros::Publisher plan_pub_; >>> - bool initialized_, allow_unknown_; >>> + bool initialized_, allow_unknown_, clear_goal_position_; >>> >>> >>> private: >>> Index: src/navfn_ros.cpp >>> =================================================================== >>> --- src/navfn_ros.cpp (revision 34269) >>> +++ src/navfn_ros.cpp (working copy) >>> @@ -64,9 +64,10 @@ >>> plan_pub_ = private_nh.advertise("plan", 1); >>> >>> private_nh.param("allow_unknown", allow_unknown_, true); >>> + private_nh.param("clear_goal_position", clear_goal_position_, >>> false); >>> private_nh.param("planner_window_x", planner_window_x_, 0.0); >>> private_nh.param("planner_window_y", planner_window_y_, 0.0); >>> - >>> + >>> double costmap_pub_freq; >>> private_nh.param("planner_costmap_publish_frequency", >>> costmap_pub_freq, 0.0); >>> >>> @@ -252,17 +253,33 @@ >>> tf::poseStampedMsgToTF(start, start_pose); >>> clearRobotCell(start_pose, mx, my); >>> >>> - //make sure to resize the underlying array that Navfn uses >>> - planner_->setNavArr(costmap_.getSizeInCellsX(), >>> costmap_.getSizeInCellsY()); >>> - planner_->setCostmap(costmap_.getCharMap(), true, allow_unknown_); >>> - >>> int map_start[2]; >>> map_start[0] = mx; >>> map_start[1] = my; >>> >>> - wx = goal.pose.position.x; >>> + wx = goal.pose.position.x; >>> wy = goal.pose.position.y; >>> >>> + if( clear_goal_position_ ) >>> + { >>> + //clear goal cell to be able to plan even if the goal is within >>> an obstacle >>> + for( float x = wx - inscribed_radius_; x <= wx + >>> inscribed_radius_; x += costmap_.getResolution() ) >>> + { >>> + for( float y = wy - inscribed_radius_; y <= wy + >>> inscribed_radius_; y += costmap_.getResolution() ) >>> + { >>> + if(!costmap_.worldToMap(x, y, mx, my)){ >>> + ROS_WARN("The goal sent to the navfn planner is off >>> the global costmap. Planning will always fail to this goal."); >>> + return false; >>> + } >>> + costmap_.setCost(mx, my, costmap_2d::FREE_SPACE); >>> + } >>> + } >>> + } >>> + >>> + //make sure to resize the underlying array that Navfn uses >>> + planner_->setNavArr(costmap_.getSizeInCellsX(), >>> costmap_.getSizeInCellsY()); >>> + planner_->setCostmap(costmap_.getCharMap(), true, allow_unknown_); >>> + >>> if(!costmap_.worldToMap(wx, wy, mx, my)){ >>> ROS_WARN("The goal sent to the navfn planner is off the global >>> costmap. Planning will always fail to this goal."); >>> return false; >>> >>> -- >>> ___________________________________________ >>> REC GmbH >>> Dr. Christian Verbeek >>> Robert-Koch-Str. 2, 82152 Planegg >>> >>> Tel: +49 89 85689672 >>> Fax: +49 89 85902327 >>> Mobile: +49 160 7056589 >>> e-mail: verbeek@servicerobotics.eu >>> >>> Geschäftsführer: Dr. Christian Verbeek >>> Registergericht: AG München (HRB 154463) >>> ____________________________________________ >>> >>> >>> _______________________________________________ >>> ros-users mailing list >>> ros-users@code.ros.org >>> https://code.ros.org/mailman/listinfo/ros-users >>> >> >> >> _______________________________________________ >> ros-users mailing list >> ros-users@code.ros.org >> https://code.ros.org/mailman/listinfo/ros-users >> >> > > > -- > Bhaskara Marthi > Research Scientist > Willow Garage Inc. > 650-475-2856 > > _______________________________________________ > ros-users mailing list > ros-users@code.ros.org > https://code.ros.org/mailman/listinfo/ros-users > >