Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17056] pkg/trunk
|
From: <is...@us...> - 2009-06-13 02:22:13
|
Revision: 17056
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17056&view=rev
Author: isucan
Date: 2009-06-13 01:37:29 +0000 (Sat, 13 Jun 2009)
Log Message:
-----------
simplifying PoseConstraint message and getting robot pose from TF rather than a localized pose
Modified Paths:
--------------
pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
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/test_path_execution.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
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/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
pkg/trunk/util/self_watch/self_watch.cpp
Modified: pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/motion_planning_msgs/msg/PoseConstraint.msg 2009-06-13 01:37:29 UTC (rev 17056)
@@ -2,8 +2,6 @@
# Since there are multiple types of constraints, the 'type' member is used
# to identify the different constraints
-Header header
-
# Constants that represent possible values for type. A position and an
# orientation constant can be combined (by adding).
@@ -31,16 +29,9 @@
# The robot link this constraint refers to
string link_name
-# The desired pose of the robot link (in the robot frame)
-float64 x
-float64 y
-float64 z
+# The desired pose of the robot link
+robot_msgs/PoseStamped pose
-# euler angles around YXZ
-float64 roll # around Z axis
-float64 pitch # around X axis
-float64 yaw # around Y axis
-
# the allowed difference (square of euclidian distance) for position
float64 position_distance
Modified: pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/ompl_planning/src/display_planner_collision_model.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -98,7 +98,7 @@
id_ = 0;
visualizationMarkerPublisher_ = nh_.advertise<visualization_msgs::Marker>("visualization_marker", 10240);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, false);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_);
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&DisplayPlannerCollisionModel::afterWorldUpdate, this, _1));
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -57,7 +57,7 @@
{
// register with ROS
m_collisionModels = new planning_environment::CollisionModels("robot_description");
- m_collisionSpaceMonitor = new planning_environment::CollisionSpaceMonitor(m_collisionModels, false);
+ m_collisionSpaceMonitor = new planning_environment::CollisionSpaceMonitor(m_collisionModels);
m_collisionSpaceMonitor->setOnAfterMapUpdateCallback(boost::bind(&KinematicPlanning::afterWorldUpdate, this, _1));
// status info
@@ -120,16 +120,13 @@
if (execute)
{
- ROS_INFO("Working in frame %s", m_collisionSpaceMonitor->getFrameId().c_str());
+ ROS_INFO("Motion planning running in frame '%s'", m_collisionSpaceMonitor->getFrameId().c_str());
startPublishingStatus();
}
}
if (execute)
- {
- ROS_INFO("Motion planning is now available.");
ros::spin();
- }
else
if (mlist.empty())
ROS_ERROR("No robot model loaded. OMPL planning node cannot start.");
@@ -150,14 +147,6 @@
ROS_WARN("Planning is not safe: kinematic state is not up to date");
return false;
}
- else
- if (m_collisionSpaceMonitor->isPoseIncluded())
- if (!m_collisionSpaceMonitor->isPoseUpdated(m_intervalKinematicState))
- {
- if (report)
- ROS_WARN("Planning is not safe: kinematic state is not up to date");
- return false;
- }
return true;
}
Modified: pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/ompl_planning/src/state_validity_monitor.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -96,7 +96,7 @@
{
stateValidityPublisher_ = nh_.advertise<std_msgs::Byte>("state_validity", 1);
collisionModels_ = new planning_environment::CollisionModels("robot_description");
- collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_, false);
+ collisionSpaceMonitor_ = new planning_environment::CollisionSpaceMonitor(collisionModels_);
if (collisionModels_->loadedModels())
{
collisionSpaceMonitor_->setOnAfterMapUpdateCallback(boost::bind(&StateValidityMonitor::afterWorldUpdate, this, _1));
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -70,7 +70,7 @@
goalA_ = true;
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, false);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
// we use the topic for sending commands to the controller, so we need to advertise it
jointCommandPublisher_ = nh_.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
@@ -146,13 +146,16 @@
req.goal_constraints.pose.resize(1);
req.goal_constraints.pose[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ + motion_planning_msgs::PoseConstraint::ORIENTATION_RP;
req.goal_constraints.pose[0].link_name = "r_gripper_r_finger_tip_link";
- req.goal_constraints.pose[0].x = 0.75025;
- req.goal_constraints.pose[0].y = -0.188;
- req.goal_constraints.pose[0].z = 0.829675;
+ req.goal_constraints.pose[0].pose.header.stamp = ros::Time::now();
+ req.goal_constraints.pose[0].pose.header.frame_id = "base_link";
+ req.goal_constraints.pose[0].pose.pose.position.x = 0.75025;
+ req.goal_constraints.pose[0].pose.pose.position.y = -0.188;
+ req.goal_constraints.pose[0].pose.pose.position.z = 0.829675;
- req.goal_constraints.pose[0].roll = 0.0;
- req.goal_constraints.pose[0].pitch = 0.0;
- req.goal_constraints.pose[0].yaw = 0.0;
+ req.goal_constraints.pose[0].pose.pose.orientation.x = 0.0;
+ req.goal_constraints.pose[0].pose.pose.orientation.y = 0.0;
+ req.goal_constraints.pose[0].pose.pose.orientation.z = 0.0;
+ req.goal_constraints.pose[0].pose.pose.orientation.w = 1.0;
req.goal_constraints.pose[0].position_distance = 0.0001;
req.goal_constraints.pose[0].orientation_distance = 0.1;
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-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -63,7 +63,7 @@
plan_id_ = -1;
robot_stopped_ = true;
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, false);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
// we use the topic for sending commands to the controller, so we need to advertise it
jointCommandPublisher_ = nh_.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
Modified: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -52,7 +52,7 @@
TestExecutionPath(void)
{
rm_ = new planning_environment::RobotModels("robot_description");
- kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_, false);
+ kmsm_ = new planning_environment::KinematicModelStateMonitor(rm_);
jointCommandPublisher_ = nh_.advertise<robot_msgs::JointTraj>("right_arm/trajectory_controller/trajectory_command", 1);
sleep_duration_ = 4;
use_topic_ = false;
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/collision_space_monitor.h 2009-06-13 01:37:29 UTC (rev 17056)
@@ -40,7 +40,6 @@
#include "planning_environment/collision_models.h"
#include "planning_environment/kinematic_model_state_monitor.h"
-#include <tf/transform_listener.h>
#include <robot_msgs/CollisionMap.h>
#include <robot_msgs/AttachedObject.h>
@@ -62,13 +61,18 @@
{
public:
- CollisionSpaceMonitor(CollisionModels *cm, bool includePose) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), includePose),
- tf_(*ros::Node::instance())
+ CollisionSpaceMonitor(CollisionModels *cm, std::string frame_id) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm), frame_id)
{
cm_ = cm;
setupCSM();
}
+ CollisionSpaceMonitor(CollisionModels *cm) : KinematicModelStateMonitor(static_cast<RobotModels*>(cm))
+ {
+ cm_ = cm;
+ setupCSM();
+ }
+
virtual ~CollisionSpaceMonitor(void)
{
}
@@ -123,7 +127,6 @@
void collisionMapCallback(const robot_msgs::CollisionMapConstPtr &collisionMap);
void attachObjectCallback(const robot_msgs::AttachedObjectConstPtr &attachedObject);
- tf::TransformListener tf_;
CollisionModels *cm_;
collision_space::EnvironmentModel *collisionSpace_;
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-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-13 01:37:29 UTC (rev 17056)
@@ -39,8 +39,8 @@
#include "planning_environment/robot_models.h"
#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
#include <robot_msgs/MechanismState.h>
-#include <robot_msgs/PoseWithCovariance.h>
#include <boost/bind.hpp>
#include <vector>
#include <string>
@@ -64,17 +64,27 @@
{
public:
- KinematicModelStateMonitor(RobotModels *rm, bool includePose)
+ KinematicModelStateMonitor(RobotModels *rm)
{
rm_ = rm;
- includePose_ = includePose;
+ includePose_ = false;
setupRSM();
}
+ KinematicModelStateMonitor(RobotModels *rm, const std::string &frame_id)
+ {
+ rm_ = rm;
+ includePose_ = true;
+ frame_id_ = frame_id;
+ setupRSM();
+ }
+
virtual ~KinematicModelStateMonitor(void)
{
if (robotState_)
delete robotState_;
+ if (tf_)
+ delete tf_;
}
/** \brief Define a callback for when the state is changed */
@@ -102,41 +112,32 @@
}
/** \brief Return the frame id of the state */
- const std::string& getFrameId(void) const;
+ const std::string& getFrameId(void) const
+ {
+ return frame_id_;
+ }
- /** \brief Return true if a pose has been received */
- bool havePose(void) const
+ /** \brief Return the robot frame id (robot without pose) */
+ const std::string& getRobotFrameId(void) const
{
- return havePose_;
+ return robot_frame_;
}
-
+
/** \brief Return true if a full mechanism state has been received */
bool haveState(void) const
{
- return haveMechanismState_;
+ return haveMechanismState_ && (!includePose_ || (includePose_ && havePose_));
}
- /** \brief Return the time of the last pose update */
- const ros::Time& lastPoseUpdate(void) const
- {
- return lastPoseUpdate_;
- }
-
/** \brief Return the time of the last state update */
const ros::Time& lastStateUpdate(void) const
{
return lastStateUpdate_;
}
-
- /** \brief Wait until a pose is received */
- void waitForPose(void) const;
/** \brief Wait until a full mechanism state is received */
void waitForState(void) const;
- /** \brief Return true if a pose has been received in the last sec seconds */
- bool isPoseUpdated(double sec) const;
-
/** \brief Return true if a full mechanism state has been received in the last sec seconds */
bool isStateUpdated(double sec) const;
@@ -152,7 +153,6 @@
protected:
void setupRSM(void);
- void localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose);
void mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState);
@@ -164,10 +164,11 @@
ros::NodeHandle nh_;
ros::Subscriber mechanismStateSubscriber_;
- ros::Subscriber localizedPoseSubscriber_;
+ tf::TransformListener *tf_;
planning_models::KinematicModel::StateParams *robotState_;
tf::Pose pose_;
+ std::string robot_frame_;
std::string frame_id_;
boost::function<void(void)> onStateUpdate_;
@@ -175,7 +176,6 @@
bool havePose_;
bool haveMechanismState_;
ros::Time lastStateUpdate_;
- ros::Time lastPoseUpdate_;
};
}
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h 2009-06-13 01:37:29 UTC (rev 17056)
@@ -59,12 +59,16 @@
{
}
+ /** Clear the stored constraint */
virtual void clear(void) = 0;
+
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc) = 0;
/** Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, const int groupID) const = 0;
+ /** Print the constraint data */
virtual void print(std::ostream &out = std::cout) const
{
}
@@ -80,13 +84,24 @@
m_kmodel = NULL;
}
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc);
+
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::JointConstraint &jc);
+
+ /** Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, const int groupID) const;
+
+ /** Clear the stored constraint */
virtual void clear(void);
- const motion_planning_msgs::JointConstraint& getConstraintMessage(void) const;
+
+ /** Print the constraint data */
virtual void print(std::ostream &out = std::cout) const;
+ /** Get the constraint message */
+ const motion_planning_msgs::JointConstraint& getConstraintMessage(void) const;
+
protected:
motion_planning_msgs::JointConstraint m_jc;
@@ -105,19 +120,39 @@
m_link = NULL;
}
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
virtual bool use(const planning_models::KinematicModel *kmodel, const ros::Message *kc);
+
+ /** This function assumes the constraint has been transformed into the proper frame, if such a transform is needed */
bool use(const planning_models::KinematicModel *kmodel, const motion_planning_msgs::PoseConstraint &pc);
+
+ /** Clear the stored constraint */
virtual void clear(void);
+
+ /** Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, int groupID) const;
+
+ /** Evaluate the distances to the position and to the orientation are given. */
void evaluate(double *distPos, double *distAng) const;
+
+ /** Decide whether the constraint is satisfied. The distances to the position and to the orientation are given. */
bool decide(double dPos, double dAng) const;
+ /** Print the constraint data */
+ void print(std::ostream &out = std::cout) const;
+
+ /** Get the constraint message */
const motion_planning_msgs::PoseConstraint& getConstraintMessage(void) const;
- void print(std::ostream &out = std::cout) const;
protected:
motion_planning_msgs::PoseConstraint m_pc;
+ double m_x;
+ double m_y;
+ double m_z;
+ double m_yaw;
+ double m_pitch;
+ double m_roll;
planning_models::KinematicModel::Link *m_link;
};
@@ -136,10 +171,19 @@
clear();
}
+ /** Clear the stored constraints */
void clear(void);
+
+ /** Add a set of joint constraints */
bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::JointConstraint> &jc);
+
+ /** Add a set of pose constraints */
bool add(const planning_models::KinematicModel *kmodel, const std::vector<motion_planning_msgs::PoseConstraint> &kc);
+
+ /** Decide whether the set of constraints is satisfied */
bool decide(const double *params, int groupID) const;
+
+ /** Print the constraint data */
void print(std::ostream &out = std::cout) const;
protected:
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/robot_models.h 2009-06-13 01:37:29 UTC (rev 17056)
@@ -85,7 +85,7 @@
loaded_models_ = false;
loadRobot();
}
-
+
virtual ~RobotModels(void)
{
}
Modified: pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/src/collision_space_monitor.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -52,6 +52,10 @@
onAfterMapUpdate_ = NULL;
onAfterAttachBody_ = NULL;
haveMap_ = false;
+
+ if (!tf_)
+ tf_ = new tf::TransformListener(*ros::Node::instance());
+
collisionSpace_ = cm_->getODECollisionModel().get();
collisionMapSubscriber_ = nh_.subscribe("collision_map", 1, &CollisionSpaceMonitor::collisionMapCallback, this);
@@ -104,7 +108,7 @@
robot_msgs::PointStamped pso;
try
{
- tf_.transformPoint(target, psi, pso);
+ tf_->transformPoint(target, psi, pso);
}
catch(...)
{
@@ -120,7 +124,7 @@
}
if (err)
- ROS_ERROR("Some errors encountered in transforming the collision map to frame %s from frame %s", target.c_str(), collisionMap->header.frame_id.c_str());
+ ROS_ERROR("Some errors encountered in transforming the collision map to frame '%s' from frame '%s'", target.c_str(), collisionMap->header.frame_id.c_str());
}
else
{
@@ -177,12 +181,12 @@
bool err = false;
try
{
- tf_.transformPoint(attachedObject->link_name, center, centerP);
+ tf_->transformPoint(attachedObject->link_name, center, centerP);
}
catch(...)
{
err = true;
- ROS_ERROR("Unable to transform object to be attached from frame %s to frame %s", attachedObject->header.frame_id.c_str(), attachedObject->link_name.c_str());
+ ROS_ERROR("Unable to transform object to be attached from frame '%s' to frame '%s'", attachedObject->header.frame_id.c_str(), attachedObject->link_name.c_str());
}
if (err)
continue;
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-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -43,6 +43,7 @@
kmodel_ = NULL;
robotState_ = NULL;
onStateUpdate_ = NULL;
+ tf_ = NULL;
havePose_ = haveMechanismState_ = false;
if (rm_->loadedModels())
{
@@ -50,7 +51,10 @@
robotState_ = kmodel_->newStateParams();
if (kmodel_->getRobotCount() > 1)
+ {
ROS_WARN("Using more than one robot. A frame_id cannot be set (there multiple frames) and pose cannot be maintained");
+ includePose_ = false;
+ }
else
{
// joints to update based on received pose
@@ -59,34 +63,23 @@
if (dynamic_cast<planning_models::KinematicModel::FloatingJoint*>(kmodel_->getRobot(0)->chain))
floatingJoint_ = kmodel_->getRobot(0)->chain->name;
+ robot_frame_ = kmodel_->getRobot(0)->chain->after->name;
+ ROS_DEBUG("Robot frame is '%s'", robot_frame_.c_str());
+
if (includePose_)
{
- localizedPoseSubscriber_ = nh_.subscribe("localized_pose", 1, &KinematicModelStateMonitor::localizedPoseCallback, this);
- ROS_DEBUG("Listening to localized pose");
+ tf_ = new tf::TransformListener(*ros::Node::instance());
+ ROS_DEBUG("Maintaining robot pose in frame '%s'", frame_id_.c_str());
}
else
- {
- frame_id_ = kmodel_->getRobot(0)->chain->after->name;
- ROS_DEBUG("Robot state frame is %s", frame_id_.c_str());
- }
+ frame_id_ = robot_frame_;
}
+
mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
ROS_DEBUG("Listening to mechanism state");
}
}
-const std::string& planning_environment::KinematicModelStateMonitor::getFrameId(void) const
-{
- if (frame_id_.empty())
- {
- if (includePose_)
- waitForPose();
- if (frame_id_.empty())
- ROS_ERROR("Cannot get frame ID for robot state");
- }
- return frame_id_;
-}
-
void planning_environment::KinematicModelStateMonitor::mechanismStateCallback(const robot_msgs::MechanismStateConstPtr &mechanismState)
{
bool change = !haveMechanismState_;
@@ -124,77 +117,67 @@
if (!haveMechanismState_)
haveMechanismState_ = robotState_->seenAll();
- if (change && onStateUpdate_ != NULL)
- onStateUpdate_();
-}
-
-void planning_environment::KinematicModelStateMonitor::localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose)
-{
- tf::PoseMsgToTF(localizedPose->pose, pose_);
- lastPoseUpdate_ = localizedPose->header.stamp;
- frame_id_ = localizedPose->header.frame_id;
-
- bool change = !havePose_;
- havePose_ = true;
-
- if (!planarJoint_.empty())
+ if (includePose_)
{
- double planar_joint[3];
- planar_joint[0] = pose_.getOrigin().x();
- planar_joint[1] = pose_.getOrigin().y();
-
- double yaw, pitch, roll;
- pose_.getBasis().getEulerZYX(yaw, pitch, roll);
- planar_joint[2] = yaw;
-
- bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoint_);
- change = change || this_changed;
+ // use tf to figure out pose
+ if (tf_->canTransform(frame_id_, robot_frame_, mechanismState->header.stamp))
+ {
+ tf::Stamped<btTransform> transf;
+ tf_->lookupTransform(frame_id_, robot_frame_, mechanismState->header.stamp, transf);
+ pose_ = transf;
+
+ if (!planarJoint_.empty())
+ {
+ double planar_joint[3];
+ planar_joint[0] = pose_.getOrigin().x();
+ planar_joint[1] = pose_.getOrigin().y();
+
+ double yaw, pitch, roll;
+ pose_.getBasis().getEulerZYX(yaw, pitch, roll);
+ planar_joint[2] = yaw;
+
+ bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoint_);
+ change = change || this_changed;
+ }
+
+ if (!floatingJoint_.empty())
+ {
+ double floating_joint[7];
+ floating_joint[0] = pose_.getOrigin().x();
+ floating_joint[1] = pose_.getOrigin().y();
+ floating_joint[2] = pose_.getOrigin().z();
+ btQuaternion q = pose_.getRotation();
+ floating_joint[3] = q.x();
+ floating_joint[4] = q.y();
+ floating_joint[5] = q.z();
+ floating_joint[6] = q.w();
+
+ bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoint_);
+ change = change || this_changed;
+ }
+
+ havePose_ = true;
+ }
+ else
+ ROS_WARN("Unable fo find transform from link '%s' to link '%s'", robot_frame_.c_str(), frame_id_.c_str());
}
- if (!floatingJoint_.empty())
- {
- double floating_joint[7];
- floating_joint[0] = pose_.getOrigin().x();
- floating_joint[1] = pose_.getOrigin().y();
- floating_joint[2] = pose_.getOrigin().z();
- btQuaternion q = pose_.getRotation();
- floating_joint[3] = q.x();
- floating_joint[4] = q.y();
- floating_joint[5] = q.z();
- floating_joint[6] = q.w();
-
- bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoint_);
- change = change || this_changed;
- }
-
if (change && onStateUpdate_ != NULL)
onStateUpdate_();
}
void planning_environment::KinematicModelStateMonitor::waitForState(void) const
{
- while (nh_.ok() && !haveMechanismState_)
+ while (nh_.ok() && !haveState())
{
ROS_INFO("Waiting for mechanism state ...");
ros::spinOnce();
ros::Duration().fromSec(0.05).sleep();
}
- if (haveMechanismState_)
+ if (haveState())
ROS_INFO("Mechanism state received!");
}
-void planning_environment::KinematicModelStateMonitor::waitForPose(void) const
-{
- while (nh_.ok() && !havePose_)
- {
- ROS_INFO("Waiting for robot pose ...");
- ros::spinOnce();
- ros::Duration().fromSec(0.05).sleep();
- }
- if (havePose_)
- ROS_INFO("Robot pose received!");
-}
-
bool planning_environment::KinematicModelStateMonitor::isStateUpdated(double sec) const
{
if (sec > 0 && lastStateUpdate_ < ros::Time::now() - ros::Duration(sec))
@@ -203,14 +186,6 @@
return true;
}
-bool planning_environment::KinematicModelStateMonitor::isPoseUpdated(double sec) const
-{
- if (sec > 0 && 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/kinematic_state_constraint_evaluator.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -35,6 +35,7 @@
/** \author Ioan Sucan */
#include "planning_environment/kinematic_state_constraint_evaluator.h"
+#include <tf/transform_datatypes.h>
#include <cassert>
bool planning_environment::JointConstraintEvaluator::use(const planning_models::KinematicModel *kmodel, const ros::Message *kc)
@@ -114,6 +115,14 @@
{
m_link = kmodel->getLink(pc.link_name);
m_pc = pc;
+
+ tf::Pose pose;
+ tf::PoseMsgToTF(m_pc.pose.pose, pose);
+ pose.getBasis().getEulerYPR(m_yaw, m_pitch, m_roll);
+ m_x = pose.getOrigin().x();
+ m_y = pose.getOrigin().y();
+ m_z = pose.getOrigin().z();
+
return true;
}
@@ -143,36 +152,36 @@
switch (m_pc.type & 0xFF)
{
case motion_planning_msgs::PoseConstraint::POSITION_XYZ:
- dx = bodyPos.getX() - m_pc.x;
- dy = bodyPos.getY() - m_pc.y;
- dz = bodyPos.getZ() - m_pc.z;
+ dx = bodyPos.getX() - m_x;
+ dy = bodyPos.getY() - m_y;
+ dz = bodyPos.getZ() - m_z;
*distPos = dx * dx + dy * dy + dz * dz;
break;
case motion_planning_msgs::PoseConstraint::POSITION_XY:
- dx = bodyPos.getX() - m_pc.x;
- dy = bodyPos.getY() - m_pc.y;
+ dx = bodyPos.getX() - m_x;
+ dy = bodyPos.getY() - m_y;
*distPos = dx * dx + dy * dy;
break;
case motion_planning_msgs::PoseConstraint::POSITION_XZ:
- dx = bodyPos.getX() - m_pc.x;
- dz = bodyPos.getZ() - m_pc.z;
+ dx = bodyPos.getX() - m_x;
+ dz = bodyPos.getZ() - m_z;
*distPos = dx * dx + dz * dz;
break;
case motion_planning_msgs::PoseConstraint::POSITION_YZ:
- dy = bodyPos.getY() - m_pc.y;
- dz = bodyPos.getZ() - m_pc.z;
+ dy = bodyPos.getY() - m_y;
+ dz = bodyPos.getZ() - m_z;
*distPos = dy * dy + dz * dz;
break;
case motion_planning_msgs::PoseConstraint::POSITION_X:
- dx = bodyPos.getX() - m_pc.x;
+ dx = bodyPos.getX() - m_x;
*distPos = dx * dx;
break;
case motion_planning_msgs::PoseConstraint::POSITION_Y:
- dy = bodyPos.getY() - m_pc.y;
+ dy = bodyPos.getY() - m_y;
*distPos = dy * dy;
break;
case motion_planning_msgs::PoseConstraint::POSITION_Z:
- dz = bodyPos.getZ() - m_pc.z;
+ dz = bodyPos.getZ() - m_z;
*distPos = dz * dz;
break;
default:
@@ -192,30 +201,30 @@
switch (m_pc.type & (~0xFF))
{
case motion_planning_msgs::PoseConstraint::ORIENTATION_RPY:
- *distAng = fabs(angles::shortest_angular_distance(roll, m_pc.roll)) +
- fabs(angles::shortest_angular_distance(pitch, m_pc.pitch)) +
- fabs(angles::shortest_angular_distance(yaw, m_pc.yaw));
+ *distAng = fabs(angles::shortest_angular_distance(roll, m_roll)) +
+ fabs(angles::shortest_angular_distance(pitch, m_pitch)) +
+ fabs(angles::shortest_angular_distance(yaw, m_yaw));
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_RP:
- *distAng = fabs(angles::shortest_angular_distance(roll, m_pc.roll)) +
- fabs(angles::shortest_angular_distance(pitch, m_pc.pitch));
+ *distAng = fabs(angles::shortest_angular_distance(roll, m_roll)) +
+ fabs(angles::shortest_angular_distance(pitch, m_pitch));
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_RY:
- *distAng = fabs(angles::shortest_angular_distance(roll, m_pc.roll)) +
- fabs(angles::shortest_angular_distance(yaw, m_pc.yaw));
+ *distAng = fabs(angles::shortest_angular_distance(roll, m_roll)) +
+ fabs(angles::shortest_angular_distance(yaw, m_yaw));
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_PY:
- *distAng = fabs(angles::shortest_angular_distance(pitch, m_pc.pitch)) +
- fabs(angles::shortest_angular_distance(yaw, m_pc.yaw));
+ *distAng = fabs(angles::shortest_angular_distance(pitch, m_pitch)) +
+ fabs(angles::shortest_angular_distance(yaw, m_yaw));
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_R:
- *distAng = fabs(angles::shortest_angular_distance(roll, m_pc.roll));
+ *distAng = fabs(angles::shortest_angular_distance(roll, m_roll));
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_P:
- *distAng = fabs(angles::shortest_angular_distance(pitch, m_pc.pitch));
+ *distAng = fabs(angles::shortest_angular_distance(pitch, m_pitch));
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_Y:
- *distAng = fabs(angles::shortest_angular_distance(yaw, m_pc.yaw));
+ *distAng = fabs(angles::shortest_angular_distance(yaw, m_yaw));
break;
default:
*distAng = 0.0;
@@ -259,30 +268,30 @@
switch (m_pc.type & 0xFF)
{
case motion_planning_msgs::PoseConstraint::POSITION_XYZ:
- out << "x = " << m_pc.x << " ";
- out << "y = " << m_pc.y << " ";
- out << "z = " << m_pc.z << " ";
+ out << "x = " << m_x << " ";
+ out << "y = " << m_y << " ";
+ out << "z = " << m_z << " ";
break;
case motion_planning_msgs::PoseConstraint::POSITION_XY:
- out << "x = " << m_pc.x << " ";
- out << "y = " << m_pc.y << " ";
+ out << "x = " << m_x << " ";
+ out << "y = " << m_y << " ";
break;
case motion_planning_msgs::PoseConstraint::POSITION_XZ:
- out << "x = " << m_pc.x << " ";
- out << "z = " << m_pc.z << " ";
+ out << "x = " << m_x << " ";
+ out << "z = " << m_z << " ";
break;
case motion_planning_msgs::PoseConstraint::POSITION_YZ:
- out << "y = " << m_pc.y << " ";
- out << "z = " << m_pc.z << " ";
+ out << "y = " << m_y << " ";
+ out << "z = " << m_z << " ";
break;
case motion_planning_msgs::PoseConstraint::POSITION_X:
- out << "x = " << m_pc.x << " ";
+ out << "x = " << m_x << " ";
break;
case motion_planning_msgs::PoseConstraint::POSITION_Y:
- out << "y = " << m_pc.y << " ";
+ out << "y = " << m_y << " ";
break;
case motion_planning_msgs::PoseConstraint::POSITION_Z:
- out << "z = " << m_pc.z << " ";
+ out << "z = " << m_z << " ";
break;
default:
break;
@@ -298,30 +307,30 @@
switch (m_pc.type & (~0xFF))
{
case motion_planning_msgs::PoseConstraint::ORIENTATION_RPY:
- out << "roll = " << m_pc.roll << " ";
- out << "pitch = " << m_pc.pitch << " ";
- out << "yaw = " << m_pc.yaw << " ";
+ out << "roll = " << m_roll << " ";
+ out << "pitch = " << m_pitch << " ";
+ out << "yaw = " << m_yaw << " ";
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_RP:
- out << "roll = " << m_pc.roll << " ";
- out << "pitch = " << m_pc.pitch << " ";
+ out << "roll = " << m_roll << " ";
+ out << "pitch = " << m_pitch << " ";
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_RY:
- out << "roll = " << m_pc.roll << " ";
- out << "yaw = " << m_pc.yaw << " ";
+ out << "roll = " << m_roll << " ";
+ out << "yaw = " << m_yaw << " ";
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_PY:
- out << "pitch = " << m_pc.pitch << " ";
- out << "yaw = " << m_pc.yaw << " ";
+ out << "pitch = " << m_pitch << " ";
+ out << "yaw = " << m_yaw << " ";
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_R:
- out << "roll = " << m_pc.roll << " ";
+ out << "roll = " << m_roll << " ";
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_P:
- out << "pitch = " << m_pc.pitch << " ";
+ out << "pitch = " << m_pitch << " ";
break;
case motion_planning_msgs::PoseConstraint::ORIENTATION_Y:
- out << "yaw = " << m_pc.yaw << " ";
+ out << "yaw = " << m_yaw << " ";
break;
default:
break;
Modified: pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -63,7 +63,7 @@
for (unsigned int i = 0 ; i < NT ; ++i)
kmodel->computeTransformsGroup(params, gid);
double fps = (double)NT / (ros::WallTime::now() - tm).toSec();
- ROS_ERROR("%f forward kinematics steps per second", fps);
+ ROS_INFO("%f forward kinematics steps per second", fps);
EXPECT_TRUE(fps > 10000.0);
}
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-06-13 00:39:21 UTC (rev 17055)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-06-13 01:37:29 UTC (rev 17056)
@@ -72,7 +72,7 @@
// create a state that can be used to monitor the
// changes in the joints of the kinematic model
- m_stateMonitor = boost::shared_ptr<planning_environment::KinematicModelStateMonitor>(new planning_environment::KinematicModelStateMonitor(m_envModels.get(), false));
+ m_stateMonitor = boost::shared_ptr<planning_environment::KinematicModelStateMonitor>(new planning_environment::KinematicModelStateMonitor(m_envModels.get()));
m_robotState = m_stateMonitor->getRobotState();
m_stateMonitor->setOnStateUpdateCallback(boost::bind(&SelfWatch::stateUpdate, this));
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
