|
From: <is...@us...> - 2009-07-09 02:06:25
|
Revision: 18537
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18537&view=rev
Author: isucan
Date: 2009-07-09 02:06:18 +0000 (Thu, 09 Jul 2009)
Log Message:
-----------
updated method for maintaining robot pose
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/mainpage.dox
pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -462,7 +462,7 @@
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
start_state[i].header.frame_id = planningMonitor_->getFrameId();
- start_state[i].header.stamp = planningMonitor_->lastStateUpdate();
+ start_state[i].header.stamp = planningMonitor_->lastMechanismStateUpdate();
start_state[i].joint_name = joints[i]->name;
st.copyParamsJoint(start_state[i].value, joints[i]->name);
}
@@ -497,7 +497,7 @@
motion_planning_msgs::JointConstraint jc;
jc.joint_name = arm_joint_names_[i];
jc.header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
- jc.header.stamp = planningMonitor_->lastStateUpdate();
+ jc.header.stamp = planningMonitor_->lastMechanismStateUpdate();
unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
for (unsigned int j = 0 ; j < u ; ++j)
{
Modified: pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/ompl_planning/src/helpers/RequestHandler.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -429,7 +429,7 @@
return;
visualization_msgs::Marker mk;
- mk.header.stamp = psetup->model->planningMonitor->lastStateUpdate();
+ mk.header.stamp = psetup->model->planningMonitor->lastMechanismStateUpdate();
mk.header.frame_id = psetup->model->planningMonitor->getFrameId();
mk.ns = nh_.getName();
mk.id = 1;
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -56,7 +56,7 @@
planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, frame);
}
else
- planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_, "torso_lift_link");
planKinematicPathService_ = nodeHandle_.advertiseService("plan_kinematic_path", &OMPLPlanning::planToGoal, this);
}
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-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -93,7 +93,7 @@
for (unsigned int i = 0 ; i < req.goal_constraints.joint_constraint.size(); ++i)
{
req.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
- req.goal_constraints.joint_constraint[i].header.frame_id = "base_link";
+ req.goal_constraints.joint_constraint[i].header.frame_id = "/base_link";
req.goal_constraints.joint_constraint[i].joint_name = names[i];
req.goal_constraints.joint_constraint[i].value.resize(1);
req.goal_constraints.joint_constraint[i].toleranceAbove.resize(1);
@@ -146,7 +146,7 @@
req.goal_constraints.joint_constraint.resize(1);
req.goal_constraints.joint_constraint[0].header.stamp = ros::Time::now();
- req.goal_constraints.joint_constraint[0].header.frame_id = "base_link";
+ req.goal_constraints.joint_constraint[0].header.frame_id = "/base_link";
req.goal_constraints.joint_constraint[0].joint_name = "base_joint";
req.goal_constraints.joint_constraint[0].value.resize(3);
req.goal_constraints.joint_constraint[0].toleranceAbove.resize(3);
@@ -166,7 +166,7 @@
// allow 1 second computation time
- req.allowed_time = 20.0;
+ req.allowed_time = 0.5;
// define the service messages
motion_planning_srvs::MotionPlan::Response res;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-07-09 02:06:18 UTC (rev 18537)
@@ -115,24 +115,35 @@
return robot_frame_;
}
- /** \brief Return true if a full mechanism state has been received */
+ /** \brief Return true if a full mechanism state has been received (including pose, if pose inclusion is enabled) */
bool haveState(void) const
{
return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
}
-
+
/** \brief Return the time of the last state update */
- const ros::Time& lastStateUpdate(void) const
+ const ros::Time& lastMechanismStateUpdate(void) const
{
- return lastStateUpdate_;
+ return lastMechanismStateUpdate_;
}
- /** \brief Wait until a full mechanism state is received */
+ /** \brief Return the time of the last pose update */
+ const ros::Time& lastPoseUpdate(void) const
+ {
+ return lastPoseUpdate_;
+ }
+
+ /** \brief Wait until a full mechanism state is received (including pose, if pose inclusion is enabled) */
void waitForState(void) const;
- /** \brief Return true if a full mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
- bool isStateUpdated(double sec) const;
+ /** \brief Return true if a mechanism state has been received in the last sec seconds. If sec < 10us, this function always returns true. */
+ bool isMechanismStateUpdated(double sec) const;
+ /** \brief Return true if a pose has been received in the last
+ sec seconds. If sec < 10us, this function always returns
+ true. */
+ bool isPoseUpdated(double sec) const;
+
/** \brief Return true if the pose is included in the state */
bool isPoseIncluded(void) const
{
@@ -170,7 +181,8 @@
bool havePose_;
bool haveMechanismState_;
- ros::Time lastStateUpdate_;
+ ros::Time lastMechanismStateUpdate_;
+ ros::Time lastPoseUpdate_;
};
}
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-07-09 02:06:18 UTC (rev 18537)
@@ -114,7 +114,8 @@
double intervalCollisionMap_;
double intervalState_;
-
+ double intervalPose_;
+
};
Modified: pkg/trunk/motion_planning/planning_environment/mainpage.dox
===================================================================
--- pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/mainpage.dox 2009-07-09 02:06:18 UTC (rev 18537)
@@ -191,6 +191,8 @@
- @b "~refresh_interval_kinematic_state"/double : if more than this interval passes, the maintained kinematic state is considered invalid
+ - @b "~refresh_interval_pose"/double : if more than this interval passes, the maintained pose is considered invalid
+
- @b "~bounding_planes"/string : a sequence of plane equations specified as "a1 b1 c1 d1 a2 b2 c2 d2 ..." where each plane is defined by the equation ax+by+cz+d=0
- @b "~box_scale"/double : boxes from the collision map are approximated with spheres using the extents of the boxes. The maximum extent of the box is multiplied by the constant specified by this parameter to obtain the radius of the sphere approximating the box
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -105,6 +105,9 @@
bool planning_environment::CollisionSpaceMonitor::isMapUpdated(double sec) const
{
+ if (!haveMap_)
+ return false;
+
// less than 10us is considered 0
if (sec > 1e-5 && lastMapUpdate_ < ros::Time::now() - ros::Duration(sec))
return false;
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -114,7 +114,7 @@
first_time = false;
- lastStateUpdate_ = mechanismState->header.stamp;
+ lastMechanismStateUpdate_ = mechanismState->header.stamp;
if (!haveMechanismState_)
haveMechanismState_ = robotState_->seenAll();
@@ -169,6 +169,7 @@
change = change || this_changed;
}
+ lastPoseUpdate_ = tm;
havePose_ = true;
}
else
@@ -188,24 +189,39 @@
while (nh_.ok() && !haveState())
{
if (s == 0)
- ROS_INFO("Waiting for mechanism state ...");
+ ROS_INFO("Waiting for robot state ...");
s = (s + 1) % 40;
ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
if (haveState())
- ROS_INFO("Mechanism state received!");
+ ROS_INFO("Robot state received!");
}
-bool planning_environment::KinematicModelStateMonitor::isStateUpdated(double sec) const
-{
+bool planning_environment::KinematicModelStateMonitor::isMechanismStateUpdated(double sec) const
+{
+ if (!haveMechanismState_)
+ return false;
+
// less than 10us is considered 0
- if (sec > 1e-5 && lastStateUpdate_ < ros::Time::now() - ros::Duration(sec))
+ if (sec > 1e-5 && lastMechanismStateUpdate_ < ros::Time::now() - ros::Duration(sec))
return false;
else
return true;
}
+bool planning_environment::KinematicModelStateMonitor::isPoseUpdated(double sec) const
+{
+ if (!havePose_)
+ return false;
+
+ // less than 10us is considered 0
+ if (sec > 1e-5 && lastPoseUpdate_ < ros::Time::now() - ros::Duration(sec))
+ return false;
+ else
+ return true;
+}
+
void planning_environment::KinematicModelStateMonitor::printRobotState(void) const
{
std::stringstream ss;
Modified: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-07-09 02:02:44 UTC (rev 18536)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-07-09 02:06:18 UTC (rev 18537)
@@ -42,6 +42,7 @@
{
nh_.param<double>("~refresh_interval_collision_map", intervalCollisionMap_, 0.0);
nh_.param<double>("~refresh_interval_kinematic_state", intervalState_, 0.0);
+ nh_.param<double>("~refresh_interval_pose", intervalPose_, 0.0);
}
bool planning_environment::PlanningMonitor::isEnvironmentSafe(void) const
@@ -52,11 +53,18 @@
return false;
}
- if (!isStateUpdated(intervalState_))
+ if (!isMechanismStateUpdated(intervalState_))
{
ROS_WARN("Planning is not safe: robot state not updated in the last %f seconds", intervalState_);
return false;
- }
+ }
+
+ if (includePose_)
+ if (!isPoseUpdated(intervalPose_))
+ {
+ ROS_WARN("Planning is not safe: robot pose not updated in the last %f seconds", intervalPose_);
+ return false;
+ }
return true;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|