[ros-users] navfn create plan even if goal hits obstacle
Christian Verbeek
verbeek at servicerobotics.eu
Fri Dec 3 09:51:36 UTC 2010
Eitan,
What parameter are you talking about? There is no such parameter for
navfn in cturtle.
Regards
Christian
Am 01.12.2010 20:18, schrieb Eitan Marder-Eppstein:
> 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 at willowgarage.com <mailto:bhaskara at willowgarage.com>> wrote:
>
>
>
> On Mon, Nov 29, 2010 at 9:43 AM, Eitan Marder-Eppstein
> <eitan at willowgarage.com <mailto:eitan at 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 at servicerobotics.eu
> <mailto:verbeek at 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 at servicerobotics.eu
> <mailto:verbeek at servicerobotics.eu>
>
> Geschäftsführer: Dr. Christian Verbeek
> Registergericht: AG München (HRB 154463)
> ____________________________________________
>
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org <mailto:ros-users at code.ros.org>
> https://code.ros.org/mailman/listinfo/ros-users
>
>
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org <mailto:ros-users at 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 at code.ros.org <mailto:ros-users at code.ros.org>
> https://code.ros.org/mailman/listinfo/ros-users
>
>
>
> _______________________________________________
> ros-users mailing list
> ros-users at code.ros.org
> https://code.ros.org/mailman/listinfo/ros-users
--
___________________________________________
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 at servicerobotics.eu
Geschäftsführer: Dr. Christian Verbeek
Registergericht: AG München (HRB 154463)
____________________________________________
-------------- next part --------------
An HTML attachment was scrubbed...
URL: <http://lists.ros.org/pipermail/ros-users/attachments/20101203/82783e69/attachment-0003.html>
More information about the ros-users
mailing list