Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16851] pkg/trunk/motion_planning/ planning_environment
|
From: <is...@us...> - 2009-06-09 02:22:57
|
Revision: 16851
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16851&view=rev
Author: isucan
Date: 2009-06-09 01:41:58 +0000 (Tue, 09 Jun 2009)
Log Message:
-----------
better handling of frame_id
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
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-09 01:36:55 UTC (rev 16850)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_model_state_monitor.h 2009-06-09 01:41:58 UTC (rev 16851)
@@ -49,7 +49,8 @@
{
/** @b KinematicModelStateMonitor is a class that monitors the robot state for the kinematic model defined in @b RobotModels
-
+ If the pose is not included, the robot state is the frame of the link it attaches to the world. If the pose is included,
+ the frame of the robot is the one in which the pose is published.
<hr>
@section topic ROS topics
@@ -98,6 +99,12 @@
return robotState_;
}
+ /** Return the frame id of the state */
+ const std::string& getFrameId(void) const
+ {
+ return frame_id_;
+ }
+
/** Return true if a pose has been received */
bool havePose(void) const
{
@@ -110,6 +117,18 @@
return haveMechanismState_;
}
+ /** Return the time of the last pose update */
+ const ros::Time& lastPoseUpdate(void) const
+ {
+ return lastPoseUpdate_;
+ }
+
+ /** Return the time of the last state update */
+ const ros::Time& lastStateUpdate(void) const
+ {
+ return lastStateUpdate_;
+ }
+
/** Wait until a pose is received */
void waitForPose(void) const;
@@ -135,8 +154,8 @@
RobotModels *rm_;
bool includePose_;
planning_models::KinematicModel *kmodel_;
- std::vector<std::string> planarJoints_;
- std::vector<std::string> floatingJoints_;
+ std::string planarJoint_;
+ std::string floatingJoint_;
ros::NodeHandle nh_;
ros::Subscriber mechanismStateSubscriber_;
@@ -144,6 +163,8 @@
planning_models::KinematicModel::StateParams *robotState_;
tf::Pose pose_;
+ std::string frame_id_;
+
boost::function<void(void)> onStateUpdate_;
bool havePose_;
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-09 01:36:55 UTC (rev 16850)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_model_state_monitor.cpp 2009-06-09 01:41:58 UTC (rev 16851)
@@ -48,16 +48,23 @@
{
kmodel_ = rm_->getKinematicModel().get();
robotState_ = kmodel_->newStateParams();
- robotState_->setInRobotFrame();
- for (unsigned int i = 0 ; i < kmodel_->getModelInfo().floatingJoints.size() ; ++i)
- floatingJoints_.push_back(kmodel_->getModelInfo().parameterName[kmodel_->getModelInfo().floatingJoints[i]]);
- for (unsigned int i = 0 ; i < kmodel_->getModelInfo().planarJoints.size() ; ++i)
- planarJoints_.push_back(kmodel_->getModelInfo().parameterName[kmodel_->getModelInfo().planarJoints[i]]);
-
+ 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");
+ else
+ {
+ // joints to update based on received pose
+ if (dynamic_cast<planning_models::KinematicModel::PlanarJoint*>(kmodel_->getRobot(0)->chain))
+ planarJoint_ = kmodel_->getRobot(0)->chain->name;
+ if (dynamic_cast<planning_models::KinematicModel::FloatingJoint*>(kmodel_->getRobot(0)->chain))
+ floatingJoint_ = kmodel_->getRobot(0)->chain->name;
+
+ if (includePose_)
+ localizedPoseSubscriber_ = nh_.subscribe("localized_pose", 1, &KinematicModelStateMonitor::localizedPoseCallback, this);
+ else
+ frame_id_ = kmodel_->getRobot(0)->chain->after->name;
+ }
mechanismStateSubscriber_ = nh_.subscribe("mechanism_state", 1, &KinematicModelStateMonitor::mechanismStateCallback, this);
- if (includePose_)
- localizedPoseSubscriber_ = nh_.subscribe("localized_pose", 1, &KinematicModelStateMonitor::localizedPoseCallback, this);
}
}
@@ -94,7 +101,7 @@
first_time = false;
- lastStateUpdate_ = ros::Time::now();
+ lastStateUpdate_ = mechanismState->header.stamp;
if (!haveMechanismState_)
haveMechanismState_ = robotState_->seenAll();
@@ -105,12 +112,13 @@
void planning_environment::KinematicModelStateMonitor::localizedPoseCallback(const robot_msgs::PoseWithCovarianceConstPtr &localizedPose)
{
tf::PoseMsgToTF(localizedPose->pose, pose_);
- lastPoseUpdate_ = ros::Time::now();
-
+ lastPoseUpdate_ = localizedPose->header.stamp;
+ frame_id_ = localizedPose->header.frame_id;
+
bool change = !havePose_;
havePose_ = true;
- if (!planarJoints_.empty())
+ if (!planarJoint_.empty())
{
double planar_joint[3];
planar_joint[0] = pose_.getOrigin().x();
@@ -120,14 +128,11 @@
pose_.getBasis().getEulerZYX(yaw, pitch, roll);
planar_joint[2] = yaw;
- for (unsigned int i = 0 ; i < planarJoints_.size() ; ++i)
- {
- bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoints_[i]);
- change = change || this_changed;
- }
+ bool this_changed = robotState_->setParamsJoint(planar_joint, planarJoint_);
+ change = change || this_changed;
}
- if (!floatingJoints_.empty())
+ if (!floatingJoint_.empty())
{
double floating_joint[7];
floating_joint[0] = pose_.getOrigin().x();
@@ -139,11 +144,8 @@
floating_joint[5] = q.z();
floating_joint[6] = q.w();
- for (unsigned int i = 0 ; i < floatingJoints_.size() ; ++i)
- {
- bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoints_[i]);
- change = change || this_changed;
- }
+ bool this_changed = robotState_->setParamsJoint(floating_joint, floatingJoint_);
+ change = change || this_changed;
}
if (change && onStateUpdate_ != NULL)
Modified: pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-09 01:36:55 UTC (rev 16850)
+++ pkg/trunk/motion_planning/planning_environment/src/robot_models.cpp 2009-06-09 01:41:58 UTC (rev 16851)
@@ -63,7 +63,7 @@
kmodel_->setVerbose(false);
kmodel_->build(*urdf_, planning_groups_);
- // make sure the kinematic model is in its own frame
+ // make sure the kinematic model is in the frame of the link that connects it to the environment
// (remove all transforms caused by planar or floating
// joints)
kmodel_->reduceToRobotFrame();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
