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
<bhaskara@willowgarage.com>
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,
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<NavFn>
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<nav_msgs::Path>("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
_______________________________________________
ros-users mailing list
ros-users@code.ros.org
https://code.ros.org/mailman/listinfo/ros-users