bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Clean up code and get regression tests working

This commit is contained in:
Wim Meeussen 2011-03-24 17:46:27 -07:00
parent 0cac9e781a
commit 2798a1691c
4 changed files with 367 additions and 614 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h
View File

@ -72,9 +72,8 @@ public:
/** Publish transforms to tf /** Publish transforms to tf
* \param joint_positions A map of joint names and joint positions. * \param joint_positions A map of joint names and joint positions.
* \param time The time at which the joint positions were recorded * \param time The time at which the joint positions were recorded
* returns true on success; return false when the robot model is empty or not all the joints in the robot model are specified in the joint map.
*/ */
bool publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time); void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
void publishFixedTransforms(); void publishFixedTransforms();
private: private:
@ -82,7 +81,7 @@ private:
std::map<std::string, SegmentPair> segments_, segments_fixed_; std::map<std::string, SegmentPair> segments_, segments_fixed_;
tf::TransformBroadcaster tf_broadcaster_, tf_broadcaster_fixed_; tf::TransformBroadcaster tf_broadcaster_;
}; };

15
robot_state_publisher/src/robot_state_publisher.cpp
View File

@ -63,11 +63,11 @@ namespace robot_state_publisher{
SegmentPair s(children[i]->second.segment, root, child.getName()); SegmentPair s(children[i]->second.segment, root, child.getName());
if (child.getJoint().getType() == KDL::Joint::None){ if (child.getJoint().getType() == KDL::Joint::None){
segments_fixed_.insert(make_pair(child.getJoint().getName(), s)); segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
ROS_INFO("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str()); ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
} }
else{ else{
segments_.insert(make_pair(child.getJoint().getName(), s)); segments_.insert(make_pair(child.getJoint().getName(), s));
ROS_INFO("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str()); ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
} }
addChildren(children[i]); addChildren(children[i]);
} }
@ -75,10 +75,10 @@ namespace robot_state_publisher{
// publish moving transforms // publish moving transforms
bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time) void RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
{ {
ROS_DEBUG("Publishing transforms for moving joints");
std::vector<tf::StampedTransform> tf_transforms; std::vector<tf::StampedTransform> tf_transforms;
tf::StampedTransform tf_transform; tf::StampedTransform tf_transform;
tf_transform.stamp_ = time; tf_transform.stamp_ = time;
@ -92,18 +92,15 @@ namespace robot_state_publisher{
tf_transforms.push_back(tf_transform); tf_transforms.push_back(tf_transform);
} }
} }
tf_broadcaster_.sendTransform(tf_transforms); tf_broadcaster_.sendTransform(tf_transforms);
return true;
} }
// publish fixed transforms // publish fixed transforms
void RobotStatePublisher::publishFixedTransforms() void RobotStatePublisher::publishFixedTransforms()
{ {
ROS_DEBUG("Publishing transforms for moving joints");
std::vector<tf::StampedTransform> tf_transforms; std::vector<tf::StampedTransform> tf_transforms;
tf::StampedTransform tf_transform; tf::StampedTransform tf_transform;
tf_transform.stamp_ = ros::Time::now()+ros::Duration(0.5); // future publish by 0.5 seconds tf_transform.stamp_ = ros::Time::now()+ros::Duration(0.5); // future publish by 0.5 seconds
@ -114,7 +111,7 @@ namespace robot_state_publisher{
tf_transform.child_frame_id_ = seg->second.tip; tf_transform.child_frame_id_ = seg->second.tip;
tf_transforms.push_back(tf_transform); tf_transforms.push_back(tf_transform);
} }
tf_broadcaster_fixed_.sendTransform(tf_transforms); tf_broadcaster_.sendTransform(tf_transforms);
} }
} }

958
robot_state_publisher/test/pr2.urdf
View File

File diff suppressed because it is too large Load Diff

3
robot_state_publisher/test/test_publisher.launch
View File

@ -1,5 +1,6 @@
<launch> <launch>
<node pkg="rosrecord" name="rosplay" type="rosplay" args="$(find robot_state_publisher)/test/joint_states_indexed.bag" /> <param name="use_sim_time" value="true" />
<node pkg="rosbag" name="rosbag" type="play" args="--clock $(find robot_state_publisher)/test/joint_states_indexed.bag" />
<node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" /> <node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" />
<param name="robot_description" textfile="$(find robot_state_publisher)/test/pr2.urdf" /> <param name="robot_description" textfile="$(find robot_state_publisher)/test/pr2.urdf" />