Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10010] pkg/trunk
|
From: <is...@us...> - 2009-01-22 23:30:14
|
Revision: 10010
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10010&view=rev
Author: isucan
Date: 2009-01-22 23:30:12 +0000 (Thu, 22 Jan 2009)
Log Message:
-----------
service to enable/disable collision checking
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
Added Paths:
-----------
pkg/trunk/robot_srvs/srv/CollisionCheckState.srv
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-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-22 23:30:12 UTC (rev 10010)
@@ -36,6 +36,7 @@
#include "kinematic_planning/KinematicStateMonitor.h"
#include <std_msgs/PointCloud.h>
+#include <robot_srvs/CollisionCheckState.h>
#include <collision_space/environmentODE.h>
#include <collision_map/CollisionMap.h>
@@ -103,6 +104,8 @@
m_collisionSpace->addStaticPlane(0.0, 0.0, 1.0, -0.01);
m_node->subscribe("collision_map", m_collisionMap, &CollisionSpaceMonitor::collisionMapCallback, this, 1);
+ m_node->advertiseService("set_collision_state", &CollisionSpaceMonitor::setCollisionState, this);
+
}
virtual ~CollisionSpaceMonitor(void)
@@ -114,6 +117,22 @@
}
}
+ bool setCollisionState(robot_srvs::CollisionCheckState::request &req, robot_srvs::CollisionCheckState::response &res)
+ {
+ m_collisionSpace->lock();
+ int model_id = m_collisionSpace->getModelID(req.robot_name);
+ if (model_id >= 0)
+ res.value = m_collisionSpace->setCollisionCheck(model_id, req.link_name, req.value ? true : false);
+ else
+ res.value = -1;
+ m_collisionSpace->unlock();
+ if (res.value == -1)
+ ROS_WARN("Unable to change collision checking state for link '%s' on '%s'", req.link_name.c_str(), req.robot_name.c_str());
+ else
+ ROS_INFO("Collision checking for link '%s' on '%s' is now %s", req.link_name.c_str(), req.robot_name.c_str(), res.value ? "enabled" : "disabled");
+ return true;
+ }
+
virtual void setRobotDescription(robot_desc::URDF *file)
{
KinematicStateMonitor::setRobotDescription(file);
Added: pkg/trunk/robot_srvs/srv/CollisionCheckState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/CollisionCheckState.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/CollisionCheckState.srv 2009-01-22 23:30:12 UTC (rev 10010)
@@ -0,0 +1,18 @@
+# This service is used to enable / disable the collision checking for
+# various links. The returned value is the old state of the collision
+# checking
+
+# The robot which contains the link
+string robot_name
+
+# The link name
+string link_name
+
+# The new state for collision checking on this link
+byte value
+
+---
+
+# If succesful, the old state of collision checking (0 or 1)
+# If not succesful, -1
+byte value
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-22 23:30:12 UTC (rev 10010)
@@ -99,13 +99,19 @@
/** Add a group of links to be checked for self collision */
virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links) = 0;
-
+
+ /** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
+ virtual int setCollisionCheck(unsigned int model_id, std::string &link, bool state) = 0;
+
/** Get the number of loaded models */
unsigned int getModelCount(void) const;
/** Get a specific model */
planning_models::KinematicModel* getRobotModel(unsigned int model_id) const;
+ /** Get the model ID based on the model (robot) name; returns -1 if model not found. */
+ int getModelID(const std::string& robot_name) const;
+
/** Provide interface to a lock. Use carefully! */
void lock(void);
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-22 23:30:12 UTC (rev 10010)
@@ -106,8 +106,8 @@
/** Add a group of links to be checked for self collision */
virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
- /** Enable/Disable collision checking for specific links */
- virtual void setCollisionCheck(unsigned int model_id, std::string &link, bool state);
+ /** Enable/Disable collision checking for specific links. Return the previous value of the state (1 or 0) if succesful; -1 otherwise */
+ virtual int setCollisionCheck(unsigned int model_id, std::string &link, bool state);
protected:
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-22 23:30:12 UTC (rev 10010)
@@ -69,6 +69,14 @@
return m_models.size();
}
+int collision_space::EnvironmentModel::getModelID(const std::string& robot_name) const
+{
+ for (unsigned int i = 0 ; i < m_models.size() ; ++i)
+ if (m_models[i]->name == robot_name)
+ return i;
+ return -1;
+}
+
planning_models::KinematicModel* collision_space::EnvironmentModel::getRobotModel(unsigned int model_id) const
{
return m_models[model_id];
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-22 23:11:39 UTC (rev 10009)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-22 23:30:12 UTC (rev 10010)
@@ -465,17 +465,20 @@
}
}
-void collision_space::EnvironmentModelODE::setCollisionCheck(unsigned int model_id, std::string &link, bool state)
+int collision_space::EnvironmentModelODE::setCollisionCheck(unsigned int model_id, std::string &link, bool state)
{
+ int result = -1;
if (model_id < m_modelsGeom.size())
{
for (unsigned int j = 0 ; j < m_modelsGeom[model_id].linkGeom.size() ; ++j)
{
if (m_modelsGeom[model_id].linkGeom[j]->link->name == link)
{
+ result = m_modelsGeom[model_id].linkGeom[j]->enabled ? 1 : 0;
m_modelsGeom[model_id].linkGeom[j]->enabled = state;
break;
}
}
}
+ return result;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
