Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16694] pkg/trunk
|
From: <ei...@us...> - 2009-06-04 01:22:45
|
Revision: 16694
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16694&view=rev
Author: eitanme
Date: 2009-06-04 00:29:45 +0000 (Thu, 04 Jun 2009)
Log Message:
-----------
Changing the name of the nav package to move_base as well as changing the name of the test_nav package to move_base_stage
Modified Paths:
--------------
pkg/trunk/demos/2dnav_erratic/manifest.xml
pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch
pkg/trunk/demos/2dnav_gazebo/manifest.xml
pkg/trunk/demos/2dnav_pr2/manifest.xml
pkg/trunk/demos/2dnav_pr2/move_base/move_base.xml
pkg/trunk/demos/2dnav_pr2/move_base/move_base_voxel.xml
pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local.xml
pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local_voxel.xml
pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml
pkg/trunk/demos/erratic_gazebo/manifest.xml
pkg/trunk/demos/milestone2/milestone2_tests/manifest.xml
pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch
pkg/trunk/highlevel/executive_python/manifest.xml
pkg/trunk/highlevel/move_base/CMakeLists.txt
pkg/trunk/highlevel/move_base/manifest.xml
pkg/trunk/highlevel/move_base/src/move_base.cpp
pkg/trunk/highlevel/move_base/src/move_base_local.cpp
pkg/trunk/highlevel/move_base_stage/manifest.xml
pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch
pkg/trunk/highlevel/move_base_stage/move_base/move_base_multi_robot.launch
pkg/trunk/highlevel/move_base_stage/move_base_local/move_base_local.launch
pkg/trunk/highlevel/move_base_stage/move_base_maze/move_base_maze.launch
pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
pkg/trunk/nav/people_aware_nav/manifest.xml
Added Paths:
-----------
pkg/trunk/highlevel/move_base/
pkg/trunk/highlevel/move_base/include/move_base/
pkg/trunk/highlevel/move_base/include/move_base/move_base.h
pkg/trunk/highlevel/move_base/include/move_base/move_base_local.h
pkg/trunk/highlevel/move_base_stage/
Removed Paths:
-------------
pkg/trunk/highlevel/move_base/include/nav/
pkg/trunk/highlevel/nav/
pkg/trunk/highlevel/test_nav/
Modified: pkg/trunk/demos/2dnav_erratic/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_erratic/manifest.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/2dnav_erratic/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -10,5 +10,5 @@
<depend package="sicktoolbox_wrapper"/>
<depend package="map_server"/>
<depend package="wavefront"/>
- <depend package="nav"/>
+ <depend package="move_base"/>
</package>
Modified: pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch
===================================================================
--- pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/2dnav_erratic/move_base/move_base.launch 2009-06-04 00:29:45 UTC (rev 16694)
@@ -1,7 +1,7 @@
<launch>
<master auto="start"/>
<group name="wg">
- <node pkg="nav" type="move_base" respawn="false" name="move_base_node" output="screen">
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
<remap from="/move_base_node/activate" to="/goal" />
<remap from="~base_local_planner/global_plan" to="/gui_path" />
<remap from="~base_local_planner/local_plan" to="/local_path" />
Modified: pkg/trunk/demos/2dnav_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/2dnav_gazebo/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -7,7 +7,7 @@
<depend package="std_msgs"/>
<depend package="robot_msgs"/>
<depend package="nav_view"/>
- <depend package="nav"/>
+ <depend package="move_base"/>
<depend package="gazebo_plugin"/>
<depend package="map_server"/>
<depend package="amcl"/>
Modified: pkg/trunk/demos/2dnav_pr2/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/manifest.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/2dnav_pr2/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -15,7 +15,7 @@
<depend package="pr2_alpha"/>
<depend package="map_server"/>
<depend package="willow_maps"/>
- <depend package="nav"/>
+ <depend package="move_base"/>
<depend package="mux"/>
<depend package="backup_safetysound"/>
Modified: pkg/trunk/demos/2dnav_pr2/move_base/move_base.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/move_base.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/2dnav_pr2/move_base/move_base.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -1,7 +1,7 @@
<launch>
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
-<node pkg="nav" type="move_base" respawn="true" name="move_base" output="screen" machine="three">
+<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen" machine="three">
<!--
<remap from="cmd_vel" to="/bs" />
-->
Modified: pkg/trunk/demos/2dnav_pr2/move_base/move_base_voxel.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base/move_base_voxel.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/2dnav_pr2/move_base/move_base_voxel.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -2,7 +2,7 @@
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<node pkg="mux" type="throttle" args="3.0 /move_base/base_local_planner/costmap/voxel_grid /move_base/base_local_planner/costmap/voxel_grid_throttled" />
-<node pkg="nav" type="move_base" respawn="true" name="move_base" output="screen" machine="three">
+<node pkg="move_base" type="move_base" respawn="true" name="move_base" output="screen" machine="three">
<!--
<remap from="cmd_vel" to="/bs" />
-->
Modified: pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -1,7 +1,7 @@
<launch>
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
-<node pkg="nav" type="move_base_local" respawn="false" name="move_base_local" output="screen" machine="three">
+<node pkg="move_base" type="move_base_local" respawn="false" name="move_base_local" output="screen" machine="three">
<param name="controller_frequency" value="10.0" />
<rosparam file="$(find 2dnav_pr2)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
Modified: pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local_voxel.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local_voxel.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/2dnav_pr2/move_base_local/move_base_local_voxel.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -2,7 +2,7 @@
<include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<node pkg="mux" type="throttle" args="3.0 /move_base_local/base_local_planner/costmap/voxel_grid /move_base_local/base_local_planner/costmap/voxel_grid_throttled" />
-<node pkg="nav" type="move_base_local" respawn="false" name="move_base_local" output="screen" machine="three">
+<node pkg="move_base" type="move_base_local" respawn="false" name="move_base_local" output="screen" machine="three">
<param name="controller_frequency" value="10.0" />
<rosparam file="$(find 2dnav_pr2)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
Modified: pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml
===================================================================
--- pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/erratic_gazebo/2dnav-gazebo-simple-fake_localization.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -27,7 +27,7 @@
</node>
<!-- for moving -->
- <node pkg="nav" type="move_base" respawn="false" name="move_base" output="screen">
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="controller_frequency" value="10.0" />
<rosparam file="$(find erratic_gazebo)/costmap_common_params.yaml" command="load" ns="navfn" />
Modified: pkg/trunk/demos/erratic_gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/erratic_gazebo/manifest.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/erratic_gazebo/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -9,7 +9,7 @@
<depend package="erratic_defs"/>
<depend package="robot_mechanism_controllers"/>
- <depend package="nav"/>
+ <depend package="move_base"/>
<depend package="tf"/>
<depend package="rospy"/>
<depend package="std_msgs"/>
Modified: pkg/trunk/demos/milestone2/milestone2_tests/manifest.xml
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_tests/manifest.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/milestone2/milestone2_tests/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -9,7 +9,7 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com/wiki/milestone2_tests</url>
<depend package="topological_map"/>
- <depend package="nav"/>
+ <depend package="move_base"/>
<depend package="robot_actions"/>
<depend package="stage"/>
<depend package="roscpp"/>
Modified: pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch
===================================================================
--- pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/demos/milestone2/milestone2_tests/test/move_base.launch 2009-06-04 00:29:45 UTC (rev 16694)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
<param name="/use_sim_time" value="true"/>
- <node pkg="nav" type="move_base" respawn="false" name="move_base" >
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base" >
<param name="global_frame" value="map" />
<param name="robot_base_frame" value="base_link" />
Modified: pkg/trunk/highlevel/executive_python/manifest.xml
===================================================================
--- pkg/trunk/highlevel/executive_python/manifest.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/highlevel/executive_python/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -4,7 +4,7 @@
<license>BSD</license>
<review status="unreviewed" notes=""/>
<depend package="rospy" />
- <depend package="nav" />
+ <depend package="nav_robot_actions" />
<depend package="std_msgs" />
<depend package="robot_msgs" />
<depend package="pr2_robot_actions" />
Modified: pkg/trunk/highlevel/move_base/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/nav/CMakeLists.txt 2009-06-02 21:28:44 UTC (rev 16612)
+++ pkg/trunk/highlevel/move_base/CMakeLists.txt 2009-06-04 00:29:45 UTC (rev 16694)
@@ -2,15 +2,15 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
set(ROS_BUILD_TYPE Release)
-rospack(nav)
+rospack(move_base)
genmsg()
rospack_add_boost_directories()
# Library
-rospack_add_library(nav src/move_base.cpp)
-rospack_link_boost(nav thread)
+rospack_add_library(move_base src/move_base.cpp)
+rospack_link_boost(move_base thread)
# move_base
rospack_add_executable(bin/move_base src/move_base.cpp)
Added: pkg/trunk/highlevel/move_base/include/move_base/move_base.h
===================================================================
--- pkg/trunk/highlevel/move_base/include/move_base/move_base.h (rev 0)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base.h 2009-06-04 00:29:45 UTC (rev 16694)
@@ -0,0 +1,148 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef NAV_MOVE_BASE_ACTION_H_
+#define NAV_MOVE_BASE_ACTION_H_
+#include <robot_actions/action.h>
+#include <robot_actions/action_runner.h>
+#include <nav_robot_actions/MoveBaseState.h>
+#include <robot_msgs/PoseStamped.h>
+#include <ros/ros.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <costmap_2d/costmap_2d.h>
+#include <costmap_2d/rate.h>
+#include <navfn/navfn_ros.h>
+#include <base_local_planner/trajectory_planner_ros.h>
+#include <vector>
+#include <string>
+#include <nav_srvs/Plan.h>
+#include <visualization_msgs/Marker.h>
+
+namespace move_base {
+ /**
+ * @class MoveBase
+ * @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
+ */
+ class MoveBase : public robot_actions::Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped> {
+ public:
+ /**
+ * @brief Constructor for the actions
+ * @param ros_node A reference to the ros node used
+ * @param tf A reference to a TransformListener
+ * @return
+ */
+ MoveBase(ros::Node& ros_node, tf::TransformListener& tf);
+
+ /**
+ * @brief Destructor - Cleans up
+ */
+ virtual ~MoveBase();
+
+ /**
+ * @brief Runs whenever a new goal is sent to the move_base
+ * @param goal The goal to pursue
+ * @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
+ * @return The result of the execution, ie: Success, Preempted, Aborted, etc.
+ */
+ virtual robot_actions::ResultStatus execute(const robot_msgs::PoseStamped& goal, robot_msgs::PoseStamped& feedback);
+
+ private:
+ /**
+ * @brief A service call that can be made when the action is inactive that will return a plan
+ * @param req The goal request
+ * @param resp The plan request
+ * @return True if planning succeeded, false otherwise
+ */
+ bool planService(nav_srvs::Plan::Request &req, nav_srvs::Plan::Response &resp);
+
+ /**
+ * @brief Make a new global plan
+ * @param goal The goal to plan to
+ */
+ void makePlan(const robot_msgs::PoseStamped& goal);
+
+ bool escape(double escape_dist, unsigned int max_attempts, const robot_msgs::PoseStamped& robot_pose);
+
+ /**
+ * @brief Publish a goal to the visualizer
+ * @param goal The goal to visualize
+ */
+ void publishGoal(const robot_msgs::PoseStamped& goal);
+
+ /**
+ * @brief Get the current pose of the robot in the specified frame
+ * @param frame The frame to get the pose in
+ * @param pose The pose returned
+ */
+ void getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose);
+
+ /**
+ * @brief Resets the costmaps to the static map outside a given window
+ */
+ void resetCostmaps(double size_x, double size_y);
+
+ void clearCostmapWindows(double size_x, double size_y);
+
+ void resetState();
+
+ bool tryPlan(robot_msgs::PoseStamped goal);
+
+ ros::Node& ros_node_;
+ tf::TransformListener& tf_;
+ bool run_planner_;
+ base_local_planner::TrajectoryPlannerROS* tc_;
+ costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;
+ costmap_2d::Costmap2D planner_costmap_, controller_costmap_;
+
+ navfn::NavfnROS* planner_;
+ std::vector<robot_msgs::PoseStamped> global_plan_;
+ std::vector<robot_msgs::Point> footprint_;
+ std::string robot_base_frame_, global_frame_;
+ bool valid_plan_, new_plan_;
+ boost::recursive_mutex lock_;
+ robot_msgs::PoseStamped goal_;
+
+ tf::Stamped<tf::Pose> global_pose_;
+ double controller_frequency_, inscribed_radius_, circumscribed_radius_, planner_patience_, controller_patience_, clearing_radius_;
+ bool attempted_rotation_, attempted_costmap_reset_;
+ bool done_half_rotation_, done_full_rotation_;
+ bool escaping_;
+ ros::Time last_valid_control_;
+
+ };
+};
+#endif
+
Added: pkg/trunk/highlevel/move_base/include/move_base/move_base_local.h
===================================================================
--- pkg/trunk/highlevel/move_base/include/move_base/move_base_local.h (rev 0)
+++ pkg/trunk/highlevel/move_base/include/move_base/move_base_local.h 2009-06-04 00:29:45 UTC (rev 16694)
@@ -0,0 +1,116 @@
+/*********************************************************************
+*
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+* Author: Eitan Marder-Eppstein
+*********************************************************************/
+#ifndef NAV_MOVE_BASE_ACTION_H_
+#define NAV_MOVE_BASE_ACTION_H_
+#include <robot_actions/action.h>
+#include <robot_actions/action_runner.h>
+#include <nav_robot_actions/MoveBaseState.h>
+#include <robot_msgs/PoseStamped.h>
+#include <ros/node.h>
+#include <costmap_2d/costmap_2d_ros.h>
+#include <costmap_2d/costmap_2d.h>
+#include <base_local_planner/trajectory_planner_ros.h>
+#include <vector>
+#include <string>
+#include <visualization_msgs/Marker.h>
+
+namespace move_base {
+ /**
+ * @class MoveBaseLocal
+ * @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
+ */
+ class MoveBaseLocal : public robot_actions::Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped> {
+ public:
+ /**
+ * @brief Constructor for the actions
+ * @param ros_node A reference to the ros node used
+ * @param tf A reference to a TransformListener
+ * @return
+ */
+ MoveBaseLocal(ros::Node& ros_node, tf::TransformListener& tf);
+
+ /**
+ * @brief Destructor - Cleans up
+ */
+ virtual ~MoveBaseLocal();
+
+ /**
+ * @brief Runs whenever a new goal is sent to the move_base
+ * @param goal The goal to pursue
+ * @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
+ * @return The result of the execution, ie: Success, Preempted, Aborted, etc.
+ */
+ virtual robot_actions::ResultStatus execute(const robot_msgs::PoseStamped& goal, robot_msgs::PoseStamped& feedback);
+
+ /**
+ * @brief Publish a goal to the visualizer
+ * @param goal The goal to visualize
+ */
+ void publishGoal(const robot_msgs::PoseStamped& goal);
+
+ private:
+ /**
+ * @brief Get the current pose of the robot in the specified frame
+ * @param frame The frame to get the pose in
+ * @param pose The pose returned
+ */
+ void getRobotPose(std::string frame, tf::Stamped<tf::Pose>& pose);
+
+ /**
+ * @brief Resets the costmaps to the static map outside a given window
+ */
+ void resetCostmaps();
+ ros::Node& ros_node_;
+ tf::TransformListener& tf_;
+
+ base_local_planner::TrajectoryPlannerROS* tc_;
+ costmap_2d::Costmap2DROS* controller_costmap_ros_;
+ costmap_2d::Costmap2D controller_costmap_;
+
+ std::vector<robot_msgs::Point> footprint_;
+ std::string robot_base_frame_;
+
+ double controller_frequency_;
+
+ std::string action_name_;
+ ros::Duration controller_patience_;
+ double circumscribed_radius_;
+
+ };
+};
+#endif
+
Modified: pkg/trunk/highlevel/move_base/manifest.xml
===================================================================
--- pkg/trunk/highlevel/nav/manifest.xml 2009-06-02 21:28:44 UTC (rev 16612)
+++ pkg/trunk/highlevel/move_base/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -23,6 +23,6 @@
<depend package="nav_srvs"/>
<export>
- <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lnav"/>
+ <cpp cflags="-I${prefix}/include `rosboost-cfg --cflags`" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib `rosboost-cfg --lflags thread` -lmove_base"/>
</export>
</package>
Modified: pkg/trunk/highlevel/move_base/src/move_base.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base.cpp 2009-06-02 21:28:44 UTC (rev 16612)
+++ pkg/trunk/highlevel/move_base/src/move_base.cpp 2009-06-04 00:29:45 UTC (rev 16694)
@@ -34,7 +34,7 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
-#include <nav/move_base.h>
+#include <move_base/move_base.h>
#include <cstdlib>
#include <ctime>
@@ -43,7 +43,7 @@
using namespace navfn;
using namespace robot_actions;
-namespace nav {
+namespace move_base {
MoveBase::MoveBase(ros::Node& ros_node, tf::TransformListener& tf) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
run_planner_(true), tc_(NULL), planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),
@@ -591,7 +591,7 @@
ros::Node ros_node("move_base");
tf::TransformListener tf(ros_node, true, ros::Duration(10));
- nav::MoveBase move_base(ros_node, tf);
+ move_base::MoveBase move_base(ros_node, tf);
robot_actions::ActionRunner runner(20.0);
runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
runner.run();
Modified: pkg/trunk/highlevel/move_base/src/move_base_local.cpp
===================================================================
--- pkg/trunk/highlevel/nav/src/move_base_local.cpp 2009-06-02 21:28:44 UTC (rev 16612)
+++ pkg/trunk/highlevel/move_base/src/move_base_local.cpp 2009-06-04 00:29:45 UTC (rev 16694)
@@ -34,14 +34,14 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
-#include <nav/move_base_local.h>
+#include <move_base/move_base_local.h>
using namespace base_local_planner;
using namespace costmap_2d;
using namespace robot_actions;
using namespace nav_robot_actions;
-namespace nav {
+namespace move_base {
MoveBaseLocal::MoveBaseLocal(ros::Node& ros_node, tf::TransformListener& tf) :
Action<robot_msgs::PoseStamped, robot_msgs::PoseStamped>(ros_node.getName()), ros_node_(ros_node), tf_(tf),
tc_(NULL), controller_costmap_ros_(NULL), action_name_("move_base_local"){
@@ -254,7 +254,7 @@
ros::Node ros_node("move_base_local");
tf::TransformListener tf(ros_node, true, ros::Duration(10));
- nav::MoveBaseLocal move_base(ros_node, tf);
+ move_base::MoveBaseLocal move_base(ros_node, tf);
robot_actions::ActionRunner runner(20.0);
runner.connect<robot_msgs::PoseStamped, nav_robot_actions::MoveBaseState, robot_msgs::PoseStamped>(move_base);
runner.run();
Modified: pkg/trunk/highlevel/move_base_stage/manifest.xml
===================================================================
--- pkg/trunk/highlevel/test_nav/manifest.xml 2009-06-01 19:10:08 UTC (rev 16551)
+++ pkg/trunk/highlevel/move_base_stage/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -7,7 +7,7 @@
<review status="unreviewed" notes=""/>
<url>http://pr.willowgarage.com</url>
- <depend package="nav"/>
+ <depend package="move_base"/>
<depend package="stage"/>
<depend package="map_server"/>
<depend package="fake_localization"/>
Modified: pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base/move_base.launch 2009-06-01 19:10:08 UTC (rev 16551)
+++ pkg/trunk/highlevel/move_base_stage/move_base/move_base.launch 2009-06-04 00:29:45 UTC (rev 16694)
@@ -2,14 +2,14 @@
<master auto="start"/>
<group name="wg">
<param name="/use_sim_time" value="true"/>
- <node pkg="nav" type="move_base" respawn="false" name="move_base" output="screen">
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<param name="footprint_padding" value="0.02" />
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="navfn" />
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
- <rosparam file="$(find test_nav)/move_base/navfn_params.yaml" command="load" />
- <rosparam file="$(find test_nav)/move_base/base_local_planner_params.yaml" command="load" />
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="navfn" />
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
+ <rosparam file="$(find move_base_stage)/move_base/navfn_params.yaml" command="load" />
+ <rosparam file="$(find move_base_stage)/move_base/base_local_planner_params.yaml" command="load" />
</node>
<!--
Modified: pkg/trunk/highlevel/move_base_stage/move_base/move_base_multi_robot.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base/move_base_multi_robot.launch 2009-06-01 19:10:08 UTC (rev 16551)
+++ pkg/trunk/highlevel/move_base_stage/move_base/move_base_multi_robot.launch 2009-06-04 00:29:45 UTC (rev 16694)
@@ -13,11 +13,11 @@
<!-- BEGIN ROBOT 0 -->
<group ns="robot_0">
<param name="~tf_prefix" value="robot_0" />
- <node pkg="nav" type="move_base" respawn="false" name="move_base" output="screen">
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="navfn" />
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
- <rosparam file="$(find test_nav)/move_base/navfn_params.yaml" command="load" />
- <rosparam file="$(find test_nav)/move_base/base_local_planner_params.yaml" command="load" />
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="navfn" />
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
+ <rosparam file="$(find move_base_stage)/move_base/navfn_params.yaml" command="load" />
+ <rosparam file="$(find move_base_stage)/move_base/base_local_planner_params.yaml" command="load" />
</node>
@@ -38,11 +38,11 @@
<!-- BEGIN ROBOT 1 -->
<group ns="robot_1">
<param name="~tf_prefix" value="robot_1" />
- <node pkg="nav" type="move_base" respawn="false" name="move_base" output="screen">
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="navfn" />
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
- <rosparam file="$(find test_nav)/move_base/navfn_params.yaml" command="load" />
- <rosparam file="$(find test_nav)/move_base/base_local_planner_params.yaml" command="load" />
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="navfn" />
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
+ <rosparam file="$(find move_base_stage)/move_base/navfn_params.yaml" command="load" />
+ <rosparam file="$(find move_base_stage)/move_base/base_local_planner_params.yaml" command="load" />
</node>
Modified: pkg/trunk/highlevel/move_base_stage/move_base_local/move_base_local.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base_local/move_base_local.launch 2009-06-01 19:10:08 UTC (rev 16551)
+++ pkg/trunk/highlevel/move_base_stage/move_base_local/move_base_local.launch 2009-06-04 00:29:45 UTC (rev 16694)
@@ -2,9 +2,9 @@
<master auto="start"/>
<group name="wg">
<param name="/use_sim_time" value="true"/>
- <node pkg="nav" type="move_base_local" respawn="false" name="move_base_local" output="screen">
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
- <rosparam file="$(find test_nav)/move_base_local/base_local_planner_params.yaml" command="load" />
+ <node pkg="move_base" type="move_base_local" respawn="false" name="move_base_local" output="screen">
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
+ <rosparam file="$(find move_base_stage)/move_base_local/base_local_planner_params.yaml" command="load" />
</node>
<!--
Modified: pkg/trunk/highlevel/move_base_stage/move_base_maze/move_base_maze.launch
===================================================================
--- pkg/trunk/highlevel/test_nav/move_base_maze/move_base_maze.launch 2009-06-01 19:10:08 UTC (rev 16551)
+++ pkg/trunk/highlevel/move_base_stage/move_base_maze/move_base_maze.launch 2009-06-04 00:29:45 UTC (rev 16694)
@@ -2,7 +2,7 @@
<master auto="start"/>
<group name="wg">
<param name="/use_sim_time" value="true"/>
- <node pkg="nav" type="move_base" respawn="false" name="move_base_node" output="screen">
+ <node pkg="move_base" type="move_base" respawn="false" name="move_base_node" output="screen">
<param name="global_frame" value="map" />
<param name="robot_base_frame" value="base_link" />
@@ -10,10 +10,10 @@
<param name="circumscribed_radius" value="0.46" />
<param name="inflation_radius" value="0.55" />
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="navfn" />
- <rosparam file="$(find test_nav)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
- <rosparam file="$(find test_nav)/move_base/navfn_params.yaml" command="load" />
- <rosparam file="$(find test_nav)/move_base/base_local_planner_params.yaml" command="load" />
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="navfn" />
+ <rosparam file="$(find move_base_stage)/config/costmap_common_params.yaml" command="load" ns="base_local_planner" />
+ <rosparam file="$(find move_base_stage)/move_base/navfn_params.yaml" command="load" />
+ <rosparam file="$(find move_base_stage)/move_base/base_local_planner_params.yaml" command="load" />
</node>
<node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-maze.pgm 0.05" respawn="false" />
Modified: pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp
===================================================================
--- pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/nav/base_local_planner/src/trajectory_planner.cpp 2009-06-04 00:29:45 UTC (rev 16694)
@@ -217,7 +217,8 @@
//create and score a trajectory given the current pose of the robot and selected velocities
void TrajectoryPlanner::generateTrajectory(double x, double y, double theta, double vx, double vy,
- double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta, double impossible_cost,
+ double vtheta, double vx_samp, double vy_samp, double vtheta_samp,
+ double acc_x, double acc_y, double acc_theta, double impossible_cost,
Trajectory& traj){
double x_i = x;
double y_i = y;
@@ -298,14 +299,17 @@
//do we want to follow blindly
if(simple_attractor_){
- goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) * (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
- (y_i - global_plan_[global_plan_.size() -1].pose.position.y) * (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
+ goal_dist = (x_i - global_plan_[global_plan_.size() -1].pose.position.x) *
+ (x_i - global_plan_[global_plan_.size() -1].pose.position.x) +
+ (y_i - global_plan_[global_plan_.size() -1].pose.position.y) *
+ (y_i - global_plan_[global_plan_.size() -1].pose.position.y);
path_dist = 0.0;
}
else{
//if a point on this trajectory has no clear path to goal it is invalid
if(impossible_cost <= goal_dist || impossible_cost <= path_dist){
- ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f", goal_dist, path_dist, impossible_cost);
+ ROS_DEBUG("No path to goal with goal distance = %f, path_distance = %f and max cost = %f",
+ goal_dist, path_dist, impossible_cost);
traj.cost_ = -2.0;
return;
}
@@ -891,20 +895,26 @@
/*
//If we want to print a ppm file to draw goal dist
- printf("P3\n");
- printf("%d %d\n", map_.size_x_, map_.size_y_);
- printf("255\n");
- for(int j = map_.size_y_ - 1; j >= 0; --j){
- for(unsigned int i = 0; i < map_.size_x_; ++i){
- int g_dist = 255 - int(map_(i, j).goal_dist);
- int p_dist = 255 - int(map_(i, j).path_dist);
- if(g_dist < 0)
- g_dist = 0;
- if(p_dist < 0)
- p_dist = 0;
- printf("%d 0 %d ", g_dist, 0);
+ char buf[4096];
+ sprintf(buf, "base_local_planner.ppm");
+ FILE *fp = fopen(buf, "w");
+ if(fp){
+ fprintf(fp, "P3\n");
+ fprintf(fp, "%d %d\n", map_.size_x_, map_.size_y_);
+ fprintf(fp, "255\n");
+ for(int j = map_.size_y_ - 1; j >= 0; --j){
+ for(unsigned int i = 0; i < map_.size_x_; ++i){
+ int g_dist = 255 - int(map_(i, j).goal_dist);
+ int p_dist = 255 - int(map_(i, j).path_dist);
+ if(g_dist < 0)
+ g_dist = 0;
+ if(p_dist < 0)
+ p_dist = 0;
+ fprintf(fp, "%d 0 %d ", g_dist, 0);
+ }
+ fprintf(fp, "\n");
}
- printf("\n");
+ fclose(fp);
}
*/
Modified: pkg/trunk/nav/people_aware_nav/manifest.xml
===================================================================
--- pkg/trunk/nav/people_aware_nav/manifest.xml 2009-06-04 00:28:18 UTC (rev 16693)
+++ pkg/trunk/nav/people_aware_nav/manifest.xml 2009-06-04 00:29:45 UTC (rev 16694)
@@ -14,7 +14,7 @@
<depend package="costmap_2d"/>
<depend package="angles"/>
<depend package="navfn"/>
-<depend package="nav"/>
+<depend package="nav_srvs"/>
<depend package="robot_msgs" />
<depend package="robot_actions" />
<depend package="deprecated_msgs" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
