Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10406] pkg/trunk
|
From: <is...@us...> - 2009-01-30 22:55:26
|
Revision: 10406
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10406&view=rev
Author: isucan
Date: 2009-01-30 22:33:17 +0000 (Fri, 30 Jan 2009)
Log Message:
-----------
updated doc, some messages, added code to test the attached object functionality
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt 2009-01-30 22:33:17 UTC (rev 10406)
@@ -7,9 +7,11 @@
rospack_add_executable(kinematic_planning src/kinematic_planning.cpp)
rospack_add_executable(motion_validator src/motion_validator.cpp)
rospack_add_executable(state_validity_monitor src/state_validity_monitor.cpp)
+rospack_add_executable(display_planner_collision_model src/display_planner_collision_model.cpp)
rospack_link_boost(kinematic_planning thread)
rospack_link_boost(motion_validator thread)
rospack_link_boost(state_validity_monitor thread)
+rospack_link_boost(display_planner_collision_model thread)
add_subdirectory(test)
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-30 22:33:17 UTC (rev 10406)
@@ -56,12 +56,9 @@
@section topic ROS topics
Subscribes to (name/type):
- - @b collision_map/CollisionMap : data describing the 3D environment
- - @b attach_object/AttachedObject : data describing an object to be attached to a link
+ - @b "collision_map"/CollisionMap : data describing the 3D environment
+ - @b "attach_object"/AttachedObject : data describing an object to be attached to a link
- Additional subscriptions due to inheritance from KinematicStateMonitor:
- - @b robot_srvs/MechanismModel : position for each of the robot's joints
-
Publishes to (name/type):
- None
@@ -73,7 +70,7 @@
- None
Provides (name/type):
- - @b set_collision_state/CollisionCheckState : service to allow enabling and disabling collision checking for links
+ - @b "set_collision_state"/CollisionCheckState : service to allow enabling and disabling collision checking for links
<hr>
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/KinematicStateMonitor.h 2009-01-30 22:33:17 UTC (rev 10406)
@@ -65,8 +65,9 @@
@section topic ROS topics
Subscribes to (name/type):
- - @b robot_srvs/MechanismModel : position for each of the robot's joints
-
+ - @b "mechanism_model"/MechanismModel : position for each of the robot's joints
+ - @b "odom_combined"/PoseWithCovariance : localized robot pose
+
Publishes to (name/type):
- None
@@ -82,6 +83,9 @@
<hr>
+ @section parameters ROS parameters
+ - @b "robot_description"/string : the URDF description of the robot we are monitoring
+
**/
class KinematicStateMonitor
Added: pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp 2009-01-30 22:33:17 UTC (rev 10406)
@@ -0,0 +1,205 @@
+/*********************************************************************
+* 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 Ioan Sucan */
+
+/**
+
+@htmlinclude ../manifest.html
+
+@b DisplayPlannerCollisionModel is a node that displays the state of
+the robot's collision space, as seen by the planner
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ display_planner_collision_model [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ display_planner_collision_model robot_description:=robotdesc/pr2
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- None
+
+Publishes to (name/type):
+- @b "visualizationMarker"/VisualizationMarker
+
+<hr>
+
+@section services ROS services
+
+Uses (name/type):
+- None
+
+Provides (name/type):
+- None
+
+
+<hr>
+
+@section parameters ROS parameters
+- None
+
+**/
+
+#include "kinematic_planning/CollisionSpaceMonitor.h"
+
+#include <std_msgs/Byte.h>
+#include <robot_msgs/VisualizationMarker.h>
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <map>
+using namespace kinematic_planning;
+
+class DisplayPlannerCollisionModel : public ros::Node,
+ public CollisionSpaceMonitor
+{
+public:
+
+ DisplayPlannerCollisionModel(void) : ros::Node("display_planner_collision_model"),
+ CollisionSpaceMonitor(dynamic_cast<ros::Node*>(this)),
+ id_(0)
+ {
+ advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 10240);
+
+ advertise<robot_msgs::AttachedObject>("attach_object", 1);
+ }
+
+ virtual ~DisplayPlannerCollisionModel(void)
+ {
+ }
+
+protected:
+
+ void afterWorldUpdate(void)
+ {
+ CollisionSpaceMonitor::afterWorldUpdate();
+
+ unsigned int n = m_collisionMap.get_boxes_size();
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ sendPoint(m_collisionMap.boxes[i].center.x,
+ m_collisionMap.boxes[i].center.y,
+ m_collisionMap.boxes[i].center.z,
+ radiusOfBox(m_collisionMap.boxes[i].extents),
+ m_collisionMap.header, 1);
+ }
+ }
+
+ void afterAttachBody(planning_models::KinematicModel::Link *link)
+ {
+ rostools::Header header = m_attachedObject.header;
+ header.frame_id = link->name;
+ for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
+ {
+ planning_models::KinematicModel::Box *box = dynamic_cast<planning_models::KinematicModel::Box*>(link->attachedBodies[i]->shape);
+ if (box)
+ {
+ btVector3 &v = link->attachedBodies[i]->attachTrans.getOrigin();
+ sendPoint(v.x(), v.y(), v.z(), std::max(std::max(box->size[0], box->size[1]), box->size[2] / 2.0), header, 0);
+ }
+ }
+ }
+
+
+private:
+
+ void sendPoint(double x, double y, double z, double radius, const rostools::Header &header, int color)
+ {
+ robot_msgs::VisualizationMarker mk;
+ mk.header = header;
+
+ mk.id = id_++;
+ mk.type = robot_msgs::VisualizationMarker::SPHERE;
+ mk.action = robot_msgs::VisualizationMarker::ADD;
+ mk.x = x;
+ mk.y = y;
+ mk.z = z;
+
+ mk.roll = 0;
+ mk.pitch = 0;
+ mk.yaw = 0;
+
+ mk.xScale = radius * 2.0;
+ mk.yScale = radius * 2.0;
+ mk.zScale = radius * 2.0;
+
+ mk.alpha = 255;
+
+ if (color == 0)
+ {
+ mk.r = 255;
+ mk.g = 10;
+ mk.b = 10;
+ }
+ else
+ {
+ mk.r = 10;
+ mk.g = 255;
+ mk.b = 10;
+ }
+
+ publish("visualizationMarker", mk);
+ }
+
+ int id_;
+
+};
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ DisplayPlannerCollisionModel *disp = new DisplayPlannerCollisionModel();
+ disp->loadRobotDescription();
+
+ disp->spin();
+ disp->shutdown();
+
+
+ delete disp;
+
+ return 0;
+}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2009-01-30 22:33:17 UTC (rev 10406)
@@ -68,8 +68,8 @@
setup/configuration code, there exists a base class that defines the
functionality and an inherited class for each type of planner that can
be instantiated. The planners are associated to string names: RRT,
-LazyRRT, EST, SBL. These string names can be used for the planner_id
-component of the planning request.
+LazyRRT, EST, SBL, IKSBL. These string names can be used for the
+planner_id component of the planning request.
When checking states for validity, a resolution at which paths are
check needs to be defined. To make things easier for the user, this
@@ -80,20 +80,33 @@
\todo
- Find a better way to specify resolution for state validity
checking.
-- Move code from header files to .cpp files (maybe define a library?)
+When using replanning, this node monitors the current state of the
+robot and that of the environment. When a change is detected, the
+currently executed path is checked for validity. If the current path
+is no longer valid, the validity status is set to false and a new path
+is computed. When the new path is computed, it is sent in the status
+messsage and validity is set to true (unless no solution is found, in
+which case the status remains invalid; a change in the map or a call
+to force_replanning will make the planner try again). When the planner
+detects that the robot reached the desired position, it stops the
+replanning thread and sets the done flag to true.
+If the monitored state of the robot and of the environment is not
+updated for a while (see the @ref parameters section), the unsafe flag
+of the planning status is set to true.
+
<hr>
@section usage Usage
@verbatim
-$ kinematic_planning robot_model [standard ROS args]
+$ kinematic_planning [standard ROS args]
@endverbatim
@par Example
@verbatim
-$ kinematic_planning robotdesc/pr2
+$ kinematic_planning robot_description:=robotdesc/pr2
@endverbatim
<hr>
@@ -101,21 +114,11 @@
@section topic ROS topics
Subscribes to (name/type):
-- @b "replan_kinematic_path_state"/KinematicPlanStateRequest : given a robot model, starting and goal states,
- this service computes and recomputes a collision free path until the monitored state is actually at the goal or
- stopping is requested. Changes in the collision model trigger replanning.
+- None
-
-- @b "replan_kinematic_path_position"/KinematicPlanStateRequest : given a robot model, starting state and goal
- poses of certain links, this service computes a collision free path until the monitored state is actually at the
-goal or stopping is requested. Changes in the collision model trigger replanning.
-
-- @b "replan_stop"/Empty : signal the planner to stop replanning
-
-Additional subscriptions due to inheritance from CollisionSpaceMonitor:
-
Publishes to (name/type):
-- @b "path_to_goal"/KinematicPath : the current path to goal (published when replanning)
+- @b "kinematic_planning_status"/KinematicPlanStatus : the current path to goal (published when replanning) and the
+ status of the motion planner (whether the path is valid, complete, etc)
<hr>
@@ -125,17 +128,40 @@
- None
Provides (name/type):
+
- @b "plan_kinematic_path_state"/KinematicPlanState : given a robot model, starting and goal
-states, this service computes a collision free path
+ states, this service computes a collision free path
+
- @b "plan_kinematic_path_position"/KinematicPlanLinkPosition : given a robot model, starting
- state and goal poses of certain links, this service computes a collision free path
+ state and goal poses of certain links, this service computes a collision free path
+- @b "replan_kinematic_path_state"/KinematicReplanState : given a robot model, starting and goal states,
+ this service computes and recomputes a collision free path until the monitored state is actually at the goal or
+ stopping is requested. Changes in the collision model trigger replanning. The computed path is published
+ as part of the status message.
+- @b "replan_kinematic_path_position"/KinematicReplanLinkPosition : given a robot model, starting state and goal
+ poses of certain links, this service computes a collision free path until the monitored state is actually at the
+ goal or stopping is requested. Changes in the collision model trigger replanning.
+
+- @b "replan_force"/Empty : signal the planner to replan one more step
+
+- @b "replan_stop"/Empty : signal the planner to stop replanning
+
+
<hr>
@section parameters ROS parameters
-- None
+- @b "refresh_interval_collision_map"/double : if more than this interval passes when receiving a request for motion planning,
+ the unsafe flag is set to true
+- @b "refresh_interval_kinematic_state"/double : if more than this interval passes when receiving a request for motion planning,
+ the unsafe flag is set to true
+
+- @b "refresh_interval_base_pose"/double : if more than this interval passes when receiving a request for motion planning,
+ the unsafe flag is set to true, unless we are planning in the robot frame, in which case, this the pase pose does not matter
+ (the collision map would be in the same frame)
+
**/
#include "kinematic_planning/CollisionSpaceMonitor.h"
@@ -172,6 +198,7 @@
m_currentPlanStatus.distance = -1.0;
m_currentPlanStatus.done = 1;
m_currentPlanStatus.valid = 1;
+ m_currentPlanStatus.unsafe = 0;
m_currentlyExecutedPath.set_states_size(0);
advertiseService("replan_kinematic_path_state", &KinematicPlanning::replanToState);
@@ -439,21 +466,11 @@
bool result = false;
- if (isSafeToPlan())
- {
- result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
-
- res.value.id = -1;
- res.value.done = trivial ? 1 : 0;
- res.value.valid = res.value.path.get_states_size() > 0;
- }
- else
- {
- res.value.id = -1;
- res.value.done = 0;
- res.value.valid = 0;
- res.value.path.set_states_size(0);
- }
+ res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ result = m_requestState.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
+ res.value.id = -1;
+ res.value.done = trivial ? 1 : 0;
+ res.value.valid = res.value.path.get_states_size() > 0;
return result;
}
@@ -471,27 +488,20 @@
boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
+ bool safe = isSafeToPlan();
+
+ currentState(m_currentPlanToStateRequest.start_state);
+ m_currentlyExecutedPathStart = m_currentPlanToStateRequest.start_state;
+ m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance, trivial);
- if (isSafeToPlan())
- {
- currentState(m_currentPlanToStateRequest.start_state);
- m_currentlyExecutedPathStart = m_currentPlanToStateRequest.start_state;
- m_requestState.execute(m_models, m_currentPlanToStateRequest, solution, distance, trivial);
+ m_statusLock.lock();
+ m_currentPlanStatus.path = solution;
+ m_currentPlanStatus.distance = distance;
+ m_currentPlanStatus.done = trivial ? 1 : 0;
+ m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
+ m_currentPlanStatus.unsafe = safe ? 0 : 1;
+ m_statusLock.unlock();
- m_statusLock.lock();
- m_currentPlanStatus.path = solution;
- m_currentPlanStatus.distance = distance;
- m_currentPlanStatus.done = trivial ? 1 : 0;
- m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
- m_statusLock.unlock();
- }
- else
- {
- m_statusLock.lock();
- m_currentPlanStatus.valid = 0;
- m_statusLock.unlock();
- }
-
if (trivial)
break;
@@ -512,22 +522,13 @@
bool result = false;
- if (isSafeToPlan())
- {
- result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
-
- res.value.id = -1;
- res.value.done = trivial ? 1 : 0;
- res.value.valid = res.value.path.get_states_size() > 0;
- }
- else
- {
- res.value.id = -1;
- res.value.done = 0;
- res.value.valid = 0;
- res.value.path.set_states_size(0);
- }
+ res.value.unsafe = isSafeToPlan() ? 0 : 1;
+ result = m_requestLinkPosition.execute(m_models, req.value, res.value.path, res.value.distance, trivial);
+ res.value.id = -1;
+ res.value.done = trivial ? 1 : 0;
+ res.value.valid = res.value.path.get_states_size() > 0;
+
return result;
}
@@ -545,27 +546,20 @@
boost::mutex::scoped_lock lock(m_continueReplanningLock);
m_collisionMonitorChange = false;
double distance = 0.0;
+ bool safe = isSafeToPlan();
- if (isSafeToPlan())
- {
- currentState(m_currentPlanToPositionRequest.start_state);
- m_currentlyExecutedPathStart = m_currentPlanToPositionRequest.start_state;
- m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance, trivial);
-
- m_statusLock.lock();
- m_currentPlanStatus.path = solution;
- m_currentPlanStatus.distance = distance;
- m_currentPlanStatus.done = trivial ? 1 : 0;
- m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
- m_statusLock.unlock();
- }
- else
- {
- m_statusLock.lock();
- m_currentPlanStatus.valid = 0;
- m_statusLock.unlock();
- }
+ currentState(m_currentPlanToPositionRequest.start_state);
+ m_currentlyExecutedPathStart = m_currentPlanToPositionRequest.start_state;
+ m_requestLinkPosition.execute(m_models, m_currentPlanToPositionRequest, solution, distance, trivial);
+ m_statusLock.lock();
+ m_currentPlanStatus.path = solution;
+ m_currentPlanStatus.distance = distance;
+ m_currentPlanStatus.done = trivial ? 1 : 0;
+ m_currentPlanStatus.valid = solution.get_states_size() > 0 ? 1 : 0;
+ m_currentPlanStatus.unsafe = safe ? 0 : 1;
+ m_statusLock.unlock();
+
if (trivial)
break;
while (m_currentRequestType == R_POSITION && !m_collisionMonitorChange)
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-30 22:33:17 UTC (rev 10406)
@@ -36,7 +36,6 @@
/**
-@mainpage
@htmlinclude ../manifest.html
@@ -48,13 +47,13 @@
@section usage Usage
@verbatim
-$ motion_validator robot_model [standard ROS args]
+$ motion_validator [standard ROS args]
@endverbatim
@par Example
@verbatim
-$ motion_validator robotdesc/pr2
+$ motion_validator robot_description:=robotdesc/pr2
@endverbatim
<hr>
@@ -64,8 +63,6 @@
Subscribes to (name/type):
- None
-Additional subscriptions due to inheritance from NodeCollisionModel:
-
Publishes to (name/type):
- None
@@ -77,7 +74,7 @@
- None
Provides (name/type):
-- @b "validate_path"/KinematicPlanState : given a robot model, starting and goal states, this service computes a collision free path
+- @b "validate_path"/ValidateKinematicPath : given a robot model, starting and goal states, this service computes whether the straight path is valid
<hr>
@@ -157,7 +154,7 @@
return false;
}
- ROS_INFO("Validating path for '%s'...", req.model_id.c_str());
+ ROS_INFO("Validating direct path for '%s'...", req.model_id.c_str());
const unsigned int dim = model->si->getStateDimension();
ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-30 22:33:17 UTC (rev 10406)
@@ -36,7 +36,6 @@
/**
-@mainpage
@htmlinclude ../manifest.html
@@ -63,8 +62,6 @@
Subscribes to (name/type):
- None
-Additional subscriptions due to inheritance from CollisionSpaceMonitor:
-
Publishes to (name/type):
- @b "state_validity"/Byte : 1 if state is valid, 0 if it is invalid
@@ -104,11 +101,9 @@
StateValidityMonitor(void) : ros::Node("state_validity_monitor"),
CollisionSpaceMonitor(dynamic_cast<ros::Node*>(this)),
- last_(-1),
- id_(0)
+ last_(-1)
{
advertise<std_msgs::Byte>("state_validity", 1);
- advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 10240);
}
virtual ~StateValidityMonitor(void)
@@ -120,21 +115,6 @@
void afterWorldUpdate(void)
{
CollisionSpaceMonitor::afterWorldUpdate();
-
- for (int i = 0 ; i < id_ ; ++i)
- delPoint(i, m_collisionMap.header);
- id_ = 0;
-
- unsigned int n = m_collisionMap.get_boxes_size();
- for (unsigned int i = 0 ; i < n ; ++i)
- {
- sendPoint(m_collisionMap.boxes[i].center.x,
- m_collisionMap.boxes[i].center.y,
- m_collisionMap.boxes[i].center.z,
- radiusOfBox(m_collisionMap.boxes[i].extents),
- m_collisionMap.header);
- }
-
last_ = -1;
}
@@ -165,48 +145,7 @@
private:
- void sendPoint(double x, double y, double z, double radius, const rostools::Header &header)
- {
- robot_msgs::VisualizationMarker mk;
- mk.header = header;
-
- mk.id = id_++;
- mk.type = robot_msgs::VisualizationMarker::SPHERE;
- mk.action = robot_msgs::VisualizationMarker::ADD;
- mk.x = x;
- mk.y = y;
- mk.z = z;
-
- mk.roll = 0;
- mk.pitch = 0;
- mk.yaw = 0;
-
- mk.xScale = radius * 2.0;
- mk.yScale = radius * 2.0;
- mk.zScale = radius * 2.0;
-
- mk.alpha = 255;
- mk.r = 255;
- mk.g = 10;
- mk.b = 10;
-
- publish("visualizationMarker", mk);
- }
-
- void delPoint(int id, const rostools::Header &header)
- {
- robot_msgs::VisualizationMarker mk;
- mk.header = header;
-
- mk.id = id;
- mk.type = robot_msgs::VisualizationMarker::SPHERE;
- mk.action = robot_msgs::VisualizationMarker::DELETE;
-
- publish("visualizationMarker", mk);
- }
-
int last_;
- int id_;
};
Modified: pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg 2009-01-30 20:58:22 UTC (rev 10405)
+++ pkg/trunk/prcore/robot_msgs/msg/KinematicPlanStatus.msg 2009-01-30 22:33:17 UTC (rev 10406)
@@ -28,3 +28,6 @@
# If computing the path was not possible, this will be set to 0
# otherwise, it will be 1
byte valid
+
+# If the data the planner has when planning is stale, set this flag to 1
+byte unsafe
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
