Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18928] pkg/trunk/motion_planning/ planning_environment
|
From: <is...@us...> - 2009-07-16 00:36:57
|
Revision: 18928
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18928&view=rev
Author: isucan
Date: 2009-07-16 00:36:56 +0000 (Thu, 16 Jul 2009)
Log Message:
-----------
additional parameter for map update event
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp
Modified: pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch
===================================================================
--- pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch 2009-07-16 00:31:30 UTC (rev 18927)
+++ pkg/trunk/motion_planning/planning_environment/display_planner_collision_model.launch 2009-07-16 00:36:56 UTC (rev 18928)
@@ -3,10 +3,7 @@
<node pkg="planning_environment" type="display_planner_collision_model" respawn="false" output="screen">
<remap from="robot_description" to="robotdesc/pr2" />
<remap from="collision_map" to="collision_map_occ" />
- <remap from="collision_map_update" to="SKIP_THIS_TOPIC" />
- <param name="refresh_interval_collision_map" type="double" value="5.0" />
- <param name="refresh_interval_kinematic_state" type="double" value="1.0" />
- <param name="refresh_interval_pose" type="double" value="1.0" />
- <param name="bounding_planes" type="string" value="0 0 1 -0.01" />
+ <remap from="collision_map_update" to="collision_map_occ_update" />
+ <param name="skip_collision_map" type="bool" value="true" />
</node>
</launch>
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-16 00:31:30 UTC (rev 18927)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-16 00:36:56 UTC (rev 18928)
@@ -89,13 +89,13 @@
}
/** \brief Define a callback for before updating a map */
- void setOnBeforeMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
+ void setOnBeforeMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> &callback)
{
onBeforeMapUpdate_ = callback;
}
/** \brief Define a callback for after updating a map */
- void setOnAfterMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr)> &callback)
+ void setOnAfterMapUpdateCallback(const boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> &callback)
{
onAfterMapUpdate_ = callback;
}
@@ -144,9 +144,9 @@
tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapUpdateNotifier_;
tf::MessageNotifier<mapping_msgs::ObjectInMap> *objectInMapNotifier_;
- boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onBeforeMapUpdate_;
- boost::function<void(const mapping_msgs::CollisionMapConstPtr)> onAfterMapUpdate_;
- boost::function<void(const mapping_msgs::ObjectInMapConstPtr)> onObjectInMapUpdate_;
+ boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> onBeforeMapUpdate_;
+ boost::function<void(const mapping_msgs::CollisionMapConstPtr, bool)> onAfterMapUpdate_;
+ boost::function<void(const mapping_msgs::ObjectInMapConstPtr)> onObjectInMapUpdate_;
};
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-16 00:31:30 UTC (rev 18927)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-16 00:36:56 UTC (rev 18928)
@@ -143,7 +143,7 @@
ROS_DEBUG("Received %d points (collision map)", n);
if (onBeforeMapUpdate_ != NULL)
- onBeforeMapUpdate_(collisionMap);
+ onBeforeMapUpdate_(collisionMap, clear);
// we want to make sure the frame the robot model is kept in is the same as the frame of the collisionMap
bool transform = !frame_id_.empty() && collisionMap->header.frame_id != frame_id_;
@@ -220,7 +220,7 @@
haveMap_ = true;
if (onAfterMapUpdate_ != NULL)
- onAfterMapUpdate_(collisionMap);
+ onAfterMapUpdate_(collisionMap, clear);
}
void planning_environment::CollisionSpaceMonitor::objectInMapCallback(const mapping_msgs::ObjectInMapConstPtr &objectInMap)
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-07-16 00:31:30 UTC (rev 18927)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/display_planner_collision_model.cpp 2009-07-16 00:36:56 UTC (rev 18928)
@@ -57,10 +57,13 @@
{
visualizationMarkerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
+ nh_.param<bool>("~skip_collision_map", skip_collision_map_, false);
+
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, &tf_);
- collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&DisplayPlannerCollisionModel::afterWorldUpdate, this, _1));
+ if (!skip_collision_map_)
+ collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&DisplayPlannerCollisionModel::afterWorldUpdate, this, _1, _2));
collisionSpaceMonitor_->setOnAfterAttachBodyCallback(boost::bind(&DisplayPlannerCollisionModel::afterAttachBody, this, _1, _2));
collisionSpaceMonitor_->setOnObjectInMapUpdateCallback(boost::bind(&DisplayPlannerCollisionModel::objectInMapUpdate, this, _1));
}
@@ -100,8 +103,11 @@
visualizationMarkerPublisher_.publish(mk);
}
- void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap)
+ void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
{
+ if (!clear)
+ return;
+
unsigned int n = collisionMap->get_boxes_size();
for (unsigned int i = 0 ; i < n ; ++i)
{
@@ -203,11 +209,13 @@
visualizationMarkerPublisher_.publish(mk);
}
- ros::Publisher visualizationMarkerPublisher_;
ros::NodeHandle nh_;
tf::TransformListener tf_;
planning_environment::CollisionModels *collisionModels_;
planning_environment::CollisionSpaceMonitor *collisionSpaceMonitor_;
+ ros::Publisher visualizationMarkerPublisher_;
+ bool skip_collision_map_;
+
};
Modified: pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp 2009-07-16 00:31:30 UTC (rev 18927)
+++ pkg/trunk/motion_planning/planning_environment/src/tools/view_state_validity.cpp 2009-07-16 00:36:56 UTC (rev 18928)
@@ -61,7 +61,7 @@
stateValidityPublisher_ = nh_.advertise<std_msgs::Byte>("state_validity", 1);
planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, &tf_);
planningMonitor_->getEnvironmentModel()->setVerbose(true);
- planningMonitor_->setOnAfterMapUpdateCallback(boost::bind(&ViewStateValidity::afterWorldUpdate, this, _1));
+ planningMonitor_->setOnAfterMapUpdateCallback(boost::bind(&ViewStateValidity::afterWorldUpdate, this, _1, _2));
planningMonitor_->setOnStateUpdateCallback(boost::bind(&ViewStateValidity::stateUpdate, this));
}
}
@@ -76,7 +76,7 @@
protected:
- void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap)
+ void afterWorldUpdate(const mapping_msgs::CollisionMapConstPtr &collisionMap, bool clear)
{
last_ = -1;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
