Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19899] pkg/trunk/motion_planning/ planning_environment
|
From: <is...@us...> - 2009-07-28 21:36:18
|
Revision: 19899
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19899&view=rev
Author: isucan
Date: 2009-07-28 21:36:05 +0000 (Tue, 28 Jul 2009)
Log Message:
-----------
start/stop possible for available monitors
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h 2009-07-28 21:12:29 UTC (rev 19898)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/collision_models.h 2009-07-28 21:36:05 UTC (rev 19899)
@@ -96,11 +96,13 @@
return bullet_collision_model_;
}
+ /** \brief Get the scaling to be used for the robot parts when inserted in the collision space */
double getScale(void)
{
return scale_;
}
+ /** \brief Get the padding to be used for the robot parts when inserted in the collision space */
double getPadding(void)
{
return padd_;
@@ -117,6 +119,7 @@
double scale_;
double padd_;
+ std::vector<double> boundingPlanes_;
};
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-28 21:12:29 UTC (rev 19898)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/collision_space_monitor.h 2009-07-28 21:36:05 UTC (rev 19899)
@@ -76,6 +76,18 @@
delete collisionMapUpdateNotifier_;
}
+ /** \brief Start the environment monitor. By default, the monitor is started after creation */
+ void startEnvironmentMonitor(void);
+
+ /** \brief Stop the environment monitor. */
+ void stopEnvironmentMonitor(void);
+
+ /** \brief Check if the environment monitor is currently started */
+ bool isEnvironmentMonitorStarted(void) const
+ {
+ return envMonitorStarted_;
+ }
+
/** \brief Return the instance of the environment model maintained */
collision_space::EnvironmentModel* getEnvironmentModel(void) const
{
@@ -144,6 +156,8 @@
boost::mutex mapUpdateLock_;
double pointcloud_padd_;
+ bool envMonitorStarted_;
+
bool haveMap_;
ros::Time lastMapUpdate_;
tf::MessageNotifier<mapping_msgs::CollisionMap> *collisionMapNotifier_;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-07-28 21:12:29 UTC (rev 19898)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h 2009-07-28 21:36:05 UTC (rev 19899)
@@ -83,6 +83,18 @@
delete attachedBodyNotifier_;
}
+ /** \brief Start the state monitor. By default, the monitor is started after creation */
+ void startStateMonitor(void);
+
+ /** \brief Stop the state monitor. */
+ void stopStateMonitor(void);
+
+ /** \brief Check if the state monitor is currently started */
+ bool isStateMonitorStarted(void) const
+ {
+ return stateMonitorStarted_;
+ }
+
/** \brief Define a callback for when the state is changed */
void setOnStateUpdateCallback(const boost::function<void(void)> &callback)
{
@@ -182,6 +194,8 @@
std::string planarJoint_;
std::string floatingJoint_;
+ bool stateMonitorStarted_;
+
ros::NodeHandle nh_;
ros::Subscriber mechanismStateSubscriber_;
tf::TransformListener *tf_;
Modified: pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp 2009-07-28 21:12:29 UTC (rev 19898)
+++ pkg/trunk/motion_planning/planning_environment/src/models/collision_models.cpp 2009-07-28 21:36:05 UTC (rev 19899)
@@ -37,6 +37,8 @@
#include "planning_environment/models/collision_models.h"
#include <collision_space/environmentODE.h>
#include <collision_space/environmentBullet.h>
+#include <sstream>
+#include <vector>
void planning_environment::CollisionModels::setupModel(boost::shared_ptr<collision_space::EnvironmentModel> &model, const std::vector<std::string> &links)
{
@@ -54,6 +56,13 @@
model->addSelfCollisionGroup(scg);
}
model->updateRobotModel();
+
+ for (unsigned int i = 0 ; i < boundingPlanes_.size() / 4 ; ++i)
+ {
+ model->addPlane("bounds", boundingPlanes_[i * 4], boundingPlanes_[i * 4 + 1], boundingPlanes_[i * 4 + 2], boundingPlanes_[i * 4 + 3]);
+ ROS_INFO("Added static plane %fx + %fy + %fz + %f = 0 for model %p", boundingPlanes_[i * 4], boundingPlanes_[i * 4 + 1], boundingPlanes_[i * 4 + 2], boundingPlanes_[i * 4 + 3], model.get());
+ }
+
model->unlock();
}
@@ -65,6 +74,26 @@
void planning_environment::CollisionModels::loadCollision(const std::vector<std::string> &links)
{
+ // a list of static planes bounding the environment
+ boundingPlanes_.clear();
+
+ std::string planes;
+ nh_.param<std::string>("~bounding_planes", planes, std::string());
+
+ std::stringstream ss(planes);
+ if (!planes.empty())
+ while (ss.good() && !ss.eof())
+ {
+ double value;
+ ss >> value;
+ boundingPlanes_.push_back(value);
+ }
+ if (boundingPlanes_.size() % 4 != 0)
+ {
+ ROS_WARN("~bounding_planes must be a list of 4-tuples (a b c d) that define planes ax+by+cz+d=0");
+ boundingPlanes_.resize(boundingPlanes_.size() - (boundingPlanes_.size() % 4));
+ }
+
if (loadedModels())
{
ode_collision_model_ = boost::shared_ptr<collision_space::EnvironmentModel>(new collision_space::EnvironmentModelODE());
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-28 21:12:29 UTC (rev 19898)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/collision_space_monitor.cpp 2009-07-28 21:36:05 UTC (rev 19899)
@@ -53,6 +53,7 @@
void planning_environment::CollisionSpaceMonitor::setupCSM(void)
{
+ envMonitorStarted_ = false;
onBeforeMapUpdate_ = NULL;
onAfterMapUpdate_ = NULL;
onObjectInMapUpdate_ = NULL;
@@ -64,29 +65,15 @@
haveMap_ = false;
collisionSpace_ = cm_->getODECollisionModel().get();
+ nh_.param<double>("~pointcloud_padd", pointcloud_padd_, 0.01);
- // a list of static planes bounding the environment
- std::string planes;
- nh_.param<std::string>("~bounding_planes", planes, std::string());
- nh_.param<double>("~pointcloud_padd", pointcloud_padd_, 0.01);
+ startEnvironmentMonitor();
+}
- std::stringstream ss(planes);
- std::vector<double> planeValues;
- if (!planes.empty())
- while (ss.good() && !ss.eof())
- {
- double value;
- ss >> value;
- planeValues.push_back(value);
- }
- if (planeValues.size() % 4 != 0)
- ROS_WARN("~bounding_planes must be a list of 4-tuples (a b c d) that define planes ax+by+cz+d=0");
- else
- for (unsigned int i = 0 ; i < planeValues.size() / 4 ; ++i)
- {
- collisionSpace_->addPlane("bounds", planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
- ROS_INFO("Added static plane %fx + %fy + %fz + %f = 0", planeValues[i * 4], planeValues[i * 4 + 1], planeValues[i * 4 + 2], planeValues[i * 4 + 3]);
- }
+void planning_environment::CollisionSpaceMonitor::startEnvironmentMonitor(void)
+{
+ if (envMonitorStarted_)
+ return;
collisionMapNotifier_ = new tf::MessageNotifier<mapping_msgs::CollisionMap>(*tf_, boost::bind(&CollisionSpaceMonitor::collisionMapCallback, this, _1), "collision_map", getFrameId(), 1);
ROS_DEBUG("Listening to collision_map using message notifier with target frame %s", collisionMapNotifier_->getTargetFramesString().c_str());
@@ -96,8 +83,29 @@
objectInMapNotifier_ = new tf::MessageNotifier<mapping_msgs::ObjectInMap>(*tf_, boost::bind(&CollisionSpaceMonitor::objectInMapCallback, this, _1), "object_in_map", getFrameId(), 1024);
ROS_DEBUG("Listening to object_in_map using message notifier with target frame %s", collisionMapUpdateNotifier_->getTargetFramesString().c_str());
+
+ envMonitorStarted_ = true;
}
+void planning_environment::CollisionSpaceMonitor::stopEnvironmentMonitor(void)
+{
+ if (!envMonitorStarted_)
+ return;
+
+ delete collisionMapUpdateNotifier_;
+ collisionMapUpdateNotifier_ = NULL;
+
+ delete collisionMapNotifier_;
+ collisionMapNotifier_ = NULL;
+
+ delete objectInMapNotifier_;
+ objectInMapNotifier_ = NULL;
+
+ ROS_DEBUG("Environment state is no longer being monitored");
+
+ envMonitorStarted_ = false;
+}
+
bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
{
if (!haveMap_)
Modified: pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-07-28 21:12:29 UTC (rev 19898)
+++ pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp 2009-07-28 21:36:05 UTC (rev 19899)
@@ -41,6 +41,7 @@
void planning_environment::KinematicModelStateMonitor::setupRSM(void)
{
+ stateMonitorStarted_ = false;
pose_.setIdentity();
kmodel_ = NULL;
robotState_ = NULL;
@@ -76,16 +77,43 @@
else
frame_id_ = robot_frame_;
}
+ }
+
+ startStateMonitor();
+}
+void planning_environment::KinematicModelStateMonitor::startStateMonitor(void)
+{
+ if (stateMonitorStarted_)
+ return;
+
+ if (rm_->loadedModels())
+ {
mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
ROS_DEBUG("Listening to mechanism state");
-
+
attachedBodyNotifier_ = new tf::MessageNotifier<mapping_msgs::AttachedObject>(*tf_, boost::bind(&KinematicModelStateMonitor::attachObjectCallback, this, _1), "attach_object", "", 1);
attachedBodyNotifier_->setTargetFrame(rm_->getCollisionCheckLinks());
ROS_DEBUG("Listening to attach_object using message notifier with target frame %s", attachedBodyNotifier_->getTargetFramesString().c_str());
}
+ stateMonitorStarted_ = true;
}
+void planning_environment::KinematicModelStateMonitor::stopStateMonitor(void)
+{
+ if (!stateMonitorStarted_)
+ return;
+
+ delete attachedBodyNotifier_;
+ attachedBodyNotifier_ = NULL;
+
+ mechanismStateSubscriber_.shutdown();
+
+ ROS_DEBUG("Kinematic state is no longer being monitored");
+
+ stateMonitorStarted_ = false;
+}
+
void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const mechanism_msgs::MechanismStateConstPtr &mechanismState)
{
bool change = !haveMechanismState_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
