Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18625] pkg/trunk
|
From: <is...@us...> - 2009-07-10 18:17:56
|
Revision: 18625
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18625&view=rev
Author: isucan
Date: 2009-07-10 18:17:51 +0000 (Fri, 10 Jul 2009)
Log Message:
-----------
point <-- pointstamped in workspace bounds definition for planning
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-10 18:11:52 UTC (rev 18624)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-10 18:17:51 UTC (rev 18625)
@@ -112,11 +112,7 @@
req.params.model_id = arm_; // the model to plan for (should be defined in planning.yaml)
req.params.distance_metric = "L2Square"; // the metric to be used in the robot's state space
-
- // this volume is only needed if planar or floating joints move in the space
- req.params.volumeMin.x = req.params.volumeMin.y = req.params.volumeMin.z = 0.0;
- req.params.volumeMax.x = req.params.volumeMax.y = req.params.volumeMax.z = 0.0;
-
+
// forward the goal & path constraints
req.goal_constraints = goal.goal_constraints;
req.path_constraints = goal.path_constraints;
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg 2009-07-10 18:11:52 UTC (rev 18624)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/KinematicSpaceParameters.msg 2009-07-10 18:17:51 UTC (rev 18625)
@@ -17,8 +17,8 @@
# Lower coordinate for a box defining the volume in the workspace the
# motion planner is active in. If planning in 2D, only first 2 values
# (x, y) of the points are used.
-robot_msgs/Point volumeMin
+robot_msgs/PointStamped volumeMin
# Higher coordinate for the box described above
-robot_msgs/Point volumeMax
+robot_msgs/PointStamped volumeMax
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-10 18:11:52 UTC (rev 18624)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-10 18:17:51 UTC (rev 18625)
@@ -134,21 +134,43 @@
static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setConstraints(req.path_constraints);
/* set the workspace volume for planning */
- // only area or volume should go through
- if (SpaceInformationKinematicModel *s = dynamic_cast<SpaceInformationKinematicModel*>(psetup->si))
+
+ if (psetup->model->planningMonitor->getTransformListener()->frameExists(req.params.volumeMin.header.frame_id) &&
+ psetup->model->planningMonitor->getTransformListener()->frameExists(req.params.volumeMax.header.frame_id))
{
- s->setPlanningArea(req.params.volumeMin.x, req.params.volumeMin.y,
- req.params.volumeMax.x, req.params.volumeMax.y);
- s->setPlanningVolume(req.params.volumeMin.x, req.params.volumeMin.y, req.params.volumeMin.z,
- req.params.volumeMax.x, req.params.volumeMax.y, req.params.volumeMax.z);
+ bool err = false;
+ try
+ {
+ psetup->model->planningMonitor->getTransformListener()->transformPoint(psetup->model->planningMonitor->getFrameId(), req.params.volumeMin, req.params.volumeMin);
+ psetup->model->planningMonitor->getTransformListener()->transformPoint(psetup->model->planningMonitor->getFrameId(), req.params.volumeMax, req.params.volumeMax);
+ }
+ catch(...)
+ {
+ err = true;
+ }
+ if (err)
+ ROS_ERROR("Unable to transform workspace bounds to planning frame");
+ else
+ {
+ // only area or volume should go through
+ if (SpaceInformationKinematicModel *s = dynamic_cast<SpaceInformationKinematicModel*>(psetup->si))
+ {
+ s->setPlanningArea(req.params.volumeMin.point.x, req.params.volumeMin.point.y,
+ req.params.volumeMax.point.x, req.params.volumeMax.point.y);
+ s->setPlanningVolume(req.params.volumeMin.point.x, req.params.volumeMin.point.y, req.params.volumeMin.point.z,
+ req.params.volumeMax.point.x, req.params.volumeMax.point.y, req.params.volumeMax.point.z);
+ }
+ if (SpaceInformationDynamicModel *s = dynamic_cast<SpaceInformationDynamicModel*>(psetup->si))
+ {
+ s->setPlanningArea(req.params.volumeMin.point.x, req.params.volumeMin.point.y,
+ req.params.volumeMax.point.x, req.params.volumeMax.point.y);
+ s->setPlanningVolume(req.params.volumeMin.point.x, req.params.volumeMin.point.y, req.params.volumeMin.point.z,
+ req.params.volumeMax.point.x, req.params.volumeMax.point.y, req.params.volumeMax.point.z);
+ }
+ }
}
- if (SpaceInformationDynamicModel *s = dynamic_cast<SpaceInformationDynamicModel*>(psetup->si))
- {
- s->setPlanningArea(req.params.volumeMin.x, req.params.volumeMin.y,
- req.params.volumeMax.x, req.params.volumeMax.y);
- s->setPlanningVolume(req.params.volumeMin.x, req.params.volumeMin.y, req.params.volumeMin.z,
- req.params.volumeMax.x, req.params.volumeMax.y, req.params.volumeMax.z);
- }
+ else
+ ROS_DEBUG("No workspace bounding volume was set");
psetup->si->setStateDistanceEvaluator(psetup->sde[req.params.distance_metric]);
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-10 18:11:52 UTC (rev 18624)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-10 18:17:51 UTC (rev 18625)
@@ -133,13 +133,17 @@
req.params.distance_metric = "L2Square";
req.params.planner_id = "dynamic::KPIECE";
- req.params.volumeMin.x = -3;
- req.params.volumeMin.y = -3;
- req.params.volumeMin.z = 0;
+ req.params.volumeMin.header.frame_id = "/base_link";
+ req.params.volumeMin.header.stamp = ros::Time::now();
+ req.params.volumeMax.header = req.params.volumeMin.header;
+
+ req.params.volumeMin.point.x = -3;
+ req.params.volumeMin.point.y = -3;
+ req.params.volumeMin.point.z = 0;
- req.params.volumeMax.x = 3;
- req.params.volumeMax.y = 3;
- req.params.volumeMax.z = 0;
+ req.params.volumeMax.point.x = 3;
+ req.params.volumeMax.point.y = 3;
+ req.params.volumeMax.point.z = 0;
req.times = 1;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
