Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10138] pkg/trunk
|
From: <is...@us...> - 2009-01-24 08:56:41
|
Revision: 10138
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10138&view=rev
Author: isucan
Date: 2009-01-24 08:56:35 +0000 (Sat, 24 Jan 2009)
Log Message:
-----------
verbosity + args bugfix
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/kinematic_planning.launch
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.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
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-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-01-24 08:56:35 UTC (rev 10138)
@@ -263,8 +263,6 @@
{
}
- private:
-
double radiusOfBox(std_msgs::Point32 &point)
{
return std::max(std::max(point.x, point.y), point.z) * 1.732050808;
Modified: pkg/trunk/motion_planning/kinematic_planning/kinematic_planning.launch
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/kinematic_planning.launch 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/kinematic_planning/kinematic_planning.launch 2009-01-24 08:56:35 UTC (rev 10138)
@@ -1,4 +1,4 @@
<launch>
- <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2" respawn="true" output="screen" />
- <node pkg="kinematic_planning" type="motion_validator" args="robotdesc/pr2" respawn="true" />
+ <node pkg="kinematic_planning" type="kinematic_planning" args="robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" output="screen" />
+ <node pkg="kinematic_planning" type="motion_validator" args="robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" />
</launch>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -41,7 +41,8 @@
@htmlinclude ../manifest.html
@b MotionValidator is a node capable of verifying if a path is valid
-or not (collides or does not collide).
+or not (collides or does not collide). The path is consider to be a
+straight line between two states.
<hr>
@@ -269,7 +270,7 @@
int main(int argc, char **argv)
{
- if (argc == 2)
+ if (argc >= 2)
{
ros::init(argc, argv);
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-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/kinematic_planning/src/state_validity_monitor.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -89,6 +89,7 @@
#include "kinematic_planning/CollisionSpaceMonitor.h"
#include <std_msgs/Byte.h>
+#include <std_msgs/VisualizationMarker.h>
#include <iostream>
#include <sstream>
@@ -104,12 +105,13 @@
StateValidityMonitor(const std::string &robot_model) : ros::Node("state_validity_monitor"),
CollisionSpaceMonitor(dynamic_cast<ros::Node*>(this),
robot_model),
- last_(-1)
+ last_(-1),
+ id_(0)
{
advertise<std_msgs::Byte>("state_validity", 1);
+ advertise<std_msgs::VisualizationMarker>("visualizationMarker", 10240);
}
- /** Free the memory */
virtual ~StateValidityMonitor(void)
{
}
@@ -119,6 +121,17 @@
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);
+ }
+
last_ = -1;
}
@@ -149,7 +162,36 @@
private:
+ void sendPoint(double x, double y, double z, double radius, const rostools::Header &header)
+ {
+ std_msgs::VisualizationMarker mk;
+ mk.header = header;
+
+ mk.id = id_++;
+ mk.type = std_msgs::VisualizationMarker::SPHERE;
+ mk.action = std_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);
+ }
+
int last_;
+ int id_;
};
@@ -161,7 +203,7 @@
int main(int argc, char **argv)
{
- if (argc == 2)
+ if (argc >= 2)
{
ros::init(argc, argv);
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -242,9 +242,9 @@
{
printPath(path, distance);
sendDisplay(req.start_state, path, req.params.model_id);
- if (verifyPath(req.start_state, req.constraints, path, req.params.model_id))
- if (controller == C_ARM)
- sendArmCommand(path, req.params.model_id);
+ verifyPath(req.start_state, req.constraints, path, req.params.model_id);
+ if (controller == C_ARM)
+ sendArmCommand(path, req.params.model_id);
}
void executePath(robot_msgs::KinematicPlanStateRequest &req,
@@ -253,9 +253,9 @@
{
printPath(path, distance);
sendDisplay(req.start_state, path, req.params.model_id);
- if (verifyPath(req.start_state, req.constraints, path, req.params.model_id))
- if (controller == C_ARM)
- sendArmCommand(path, req.params.model_id);
+ verifyPath(req.start_state, req.constraints, path, req.params.model_id);
+ if (controller == C_ARM)
+ sendArmCommand(path, req.params.model_id);
}
protected:
@@ -266,10 +266,9 @@
for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
{
- traj.points[i].set_positions_size(path.states[i].get_vals_size() + 1);
+ traj.points[i].set_positions_size(path.states[i].get_vals_size());
for (unsigned int j = 0 ; j < path.states[i].get_vals_size() ; ++j)
traj.points[i].positions[j] = path.states[i].vals[j];
- traj.points[i].positions[path.states[i].get_vals_size()] = m_gripPos;
traj.points[i].time = 0.0;
}
}
@@ -372,9 +371,18 @@
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 5.0 + m_basePos[0]; req.params.volumeMax.y = 5.0 + m_basePos[1]; req.params.volumeMax.z = 0.0;
- robot_srvs::KinematicPlanState::request r;
- r.value = req;
- m_pr.performCall(r, PlanningRequest::C_ARM);
+ if (replan)
+ {
+ robot_srvs::KinematicReplanState::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
+ else
+ {
+ robot_srvs::KinematicPlanState::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
}
void runRightArmTo0(bool replan = false)
@@ -400,10 +408,19 @@
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 5.0 + m_basePos[0]; req.params.volumeMax.y = 5.0 + m_basePos[1]; req.params.volumeMax.z = 0.0;
-
- robot_srvs::KinematicPlanState::request r;
- r.value = req;
- m_pr.performCall(r, PlanningRequest::C_ARM);
+
+ if (replan)
+ {
+ robot_srvs::KinematicReplanState::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
+ else
+ {
+ robot_srvs::KinematicPlanState::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
}
void printLinkPoses(void)
@@ -461,9 +478,18 @@
req.params.volumeMax.y = 5.0 + m_basePos[1];
req.params.volumeMax.z = 0.0;
- robot_srvs::KinematicPlanLinkPosition::request r;
- r.value = req;
- m_pr.performCall(r, PlanningRequest::C_ARM);
+ if (replan)
+ {
+ robot_srvs::KinematicReplanLinkPosition::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
+ else
+ {
+ robot_srvs::KinematicPlanLinkPosition::request r;
+ r.value = req;
+ m_pr.performCall(r, PlanningRequest::C_ARM);
+ }
}
@@ -488,9 +514,9 @@
mk.pitch = 0;
mk.yaw = 0;
- mk.xScale = radius;
- mk.yScale = radius;
- mk.zScale = radius;
+ mk.xScale = radius * 2.0;
+ mk.yScale = radius * 2.0;
+ mk.zScale = radius * 2.0;
mk.alpha = 255;
mk.r = 255;
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-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-24 08:56:35 UTC (rev 10138)
@@ -38,7 +38,7 @@
#define COLLISION_SPACE_ENVIRONMENT_MODEL_
#include <planning_models/kinematic.h>
-#include "boost/thread/mutex.hpp"
+#include <boost/thread/mutex.hpp>
#include <vector>
#include <string>
@@ -68,6 +68,7 @@
EnvironmentModel(void)
{
m_selfCollision = true;
+ m_verbose = false;
}
virtual ~EnvironmentModel(void)
@@ -76,6 +77,9 @@
delete m_models[i];
}
+ /** Enable/disable verbosity */
+ void setVerbose(bool verbose);
+
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id) = 0;
@@ -128,6 +132,7 @@
boost::mutex m_lock;
bool m_selfCollision;
+ bool m_verbose;
/** List of loaded robot models */
std::vector<planning_models::KinematicModel*> m_models;
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-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -36,6 +36,11 @@
#include <collision_space/environment.h>
+void collision_space::EnvironmentModel::setVerbose(bool verbose)
+{
+ m_verbose = verbose;
+}
+
unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
{
unsigned int pos = m_models.size();
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-24 08:36:18 UTC (rev 10137)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-24 08:56:35 UTC (rev 10138)
@@ -36,6 +36,7 @@
#include <collision_space/environmentODE.h>
#include <cassert>
+#include <cstdio>
#include <algorithm>
#include <map>
@@ -364,7 +365,12 @@
dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
if (cdata.collides)
+ {
+ if (m_verbose)
+ printf("Self-collision between '%s' and '%s'\n",
+ m_modelsGeom[model_id].linkGeom[vec[j]]->link->name.c_str(), m_modelsGeom[model_id].linkGeom[vec[k]]->link->name.c_str());
goto OUT1;
+ }
}
}
}
@@ -399,7 +405,13 @@
dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
if (cdata.collides)
+ {
+ if (m_verbose)
+ printf("Collision between static body and link '%s'\n",
+ m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
goto OUT2;
+ }
+
}
}
}
@@ -415,8 +427,17 @@
if (m_modelsGeom[model_id].linkGeom[i]->enabled)
{
const unsigned int ng = m_modelsGeom[model_id].linkGeom[i]->geom.size();
- for (unsigned int ig = 0 ; ig < ng && !cdata.collides ; ++ig)
+ for (unsigned int ig = 0 ; ig < ng ; ++ig)
+ {
m_collide2.collide(m_modelsGeom[model_id].linkGeom[i]->geom[ig], reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ if (cdata.collides)
+ {
+ if (m_verbose)
+ printf("Collision between dynamic body and link '%s'\n",
+ m_modelsGeom[model_id].linkGeom[i]->link->name.c_str());
+ break;
+ }
+ }
}
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
