Changes have been pushed for the project "Fawkes Robotics Software Framework".
Gitweb: http://git.fawkesrobotics.org/fawkes.git Trac: http://trac.fawkesrobotics.org The branch, thofmann/robot_state_publisher has been created at 887ca9a31bf7a0ee6df2116d473b01a1b493728b (commit) http://git.fawkesrobotics.org/fawkes.git/thofmann/robot_state_publisher - *Log* --------------------------------------------------------------- commit 9570093a6c2f06f538330b0aae6a44b56b790712 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Fri Aug 23 15:08:15 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Mon Feb 24 13:12:04 2014 +0100 robot-publisher: add files from ros package, create plugin -imported from https://github.com/ros/robot_state_publisher http://git.fawkesrobotics.org/fawkes.git/commit/9570093 http://trac.fawkesrobotics.org/changeset/9570093 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit c981461827db1ae97cfe6cfd45c2a29f262e4299 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Mon Sep 9 14:36:50 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Mon Feb 24 13:12:04 2014 +0100 robot-publisher: adapt to fawkes-tf fawkes-tf slightly differs from ros-tf, adapt to those changes. -tf_transforms member variables are named differently -use logger->log_debug instead of ROS_DEBUG -ROS tf_broadcaster is called tf_publisher -tf_publisher doesn't support publishing arrays, iterate over array -add method transformKDLToTF (adopted from ROS geometry): transforms a KDL::Frame to a fawkes::tf::Transform http://git.fawkesrobotics.org/fawkes.git/commit/c981461 http://trac.fawkesrobotics.org/changeset/c981461 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit fef8305f7c862eeeccf4fa309caa26a597574f01 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Mon Sep 9 14:43:30 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Mon Feb 24 13:12:04 2014 +0100 robot-publisher: publish information -initialize correctly -publish static transforms every loop -publish robot description to ROS (as param) -read the URDF description file and set the ROS param /robot_description to the content of the file. -URDF description file can be set as config value (/robot_state_publisher/urdf_file) -watch blackboard for JointInterfaces and create a dynamic transform if the joint is part of the robot model. http://git.fawkesrobotics.org/fawkes.git/commit/fef8305 http://trac.fawkesrobotics.org/changeset/fef8305 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit f5c36dcad2046cbec9948e611b8ec3c61e61fa23 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Fri Sep 27 19:46:55 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Mon Feb 24 13:12:05 2014 +0100 robot-publisher: warn if joint information is incomplete -warn if a joint of the model has no JointInterface -warn if a JointInterface is removed which is part of the model -print joint information when joint is found http://git.fawkesrobotics.org/fawkes.git/commit/f5c36dc http://trac.fawkesrobotics.org/changeset/f5c36dc - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit cfb250f37ff4d440371c52864f56783d0164407a Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Fri Sep 27 19:55:24 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Mon Feb 24 13:12:05 2014 +0100 robot-publisher: change timing hook to sensor acquire hook http://git.fawkesrobotics.org/fawkes.git/commit/cfb250f http://trac.fawkesrobotics.org/changeset/cfb250f - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 38985f94d335481ce20bb08206c0d1f7b9477a0d Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Fri Sep 27 20:33:39 2013 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Mon Feb 24 13:12:05 2014 +0100 robot-publisher: remove unused function publishTransforms publishTransforms used to publish all dynamic transforms and isn't used anymore. http://git.fawkesrobotics.org/fawkes.git/commit/38985f9 http://trac.fawkesrobotics.org/changeset/38985f9 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 887ca9a31bf7a0ee6df2116d473b01a1b493728b Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Tue Feb 18 18:34:27 2014 +0100 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Mon Feb 24 13:15:02 2014 +0100 robot-publisher: adapt to use fawkes' KDL Parser instead of ROS Package The robot state publisher now doesn't need ROS anymore. http://git.fawkesrobotics.org/fawkes.git/commit/887ca9a http://trac.fawkesrobotics.org/changeset/887ca9a - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- cfg/conf.d/robot_state_publisher.yaml | 7 - src/plugins/robot_state_publisher/Makefile | 57 ++++-- .../robot_state_publisher_thread.cpp | 198 +++++--------------- .../robot_state_publisher_thread.h | 44 ++--- 4 files changed, 98 insertions(+), 208 deletions(-) delete mode 100644 cfg/conf.d/robot_state_publisher.yaml - *Diffs* ------------------------------------------------------------- - *commit* 9570093a6c2f06f538330b0aae6a44b56b790712 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Fri Aug 23 15:08:15 2013 +0200 Subject: robot-publisher: add files from ros package, create plugin src/plugins/robot_state_publisher/Makefile | 75 +++++++++ .../robot_state_publisher_plugin.cpp} | 24 ++-- .../robot_state_publisher_thread.cpp | 160 ++++++++++++++++++++ .../robot_state_publisher_thread.h | 116 ++++++++++++++ 4 files changed, 363 insertions(+), 12 deletions(-) - *commit* c981461827db1ae97cfe6cfd45c2a29f262e4299 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Mon Sep 9 14:36:50 2013 +0200 Subject: robot-publisher: adapt to fawkes-tf .../robot_state_publisher_thread.cpp | 47 +++++++++++++------- .../robot_state_publisher_thread.h | 4 +- 2 files changed, 34 insertions(+), 17 deletions(-) _Diff for modified files_: diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp index fe809ac..8748136 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp @@ -19,8 +19,8 @@ * Read the full text in the LICENSE.GPL file in the doc directory. */ -/* This code is based on ROS robot_state_publisher with the following copyright - * and license: +/* This code is based on ROS robot_state_publisher and ROS geometry + * with the following copyright and license: * Software License Agreement (BSD License) * * Copyright (c) 2008, Willow Garage, Inc. @@ -108,11 +108,11 @@ void RobotStatePublisherThread::addChildren(const KDL::SegmentMap::const_iterato SegmentPair s(children[i]->second.segment, root, child.getName()); if (child.getJoint().getType() == KDL::Joint::None){ segments_fixed_.insert(make_pair(child.getJoint().getName(), s)); - ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str()); + logger->log_debug(name(), "Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str()); } else{ segments_.insert(make_pair(child.getJoint().getName(), s)); - ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str()); + logger->log_debug(name(), "Adding moving segment from %s to %s", root.c_str(), child.getName().c_str()); } addChildren(children[i]); } @@ -122,39 +122,54 @@ void RobotStatePublisherThread::addChildren(const KDL::SegmentMap::const_iterato // publish moving transforms void RobotStatePublisherThread::publishTransforms(const map<string, double>& joint_positions, const Time& time) { - ROS_DEBUG("Publishing transforms for moving joints"); + logger->log_debug(name(), "Publishing transforms for moving joints"); std::vector<tf::StampedTransform> tf_transforms; tf::StampedTransform tf_transform; - tf_transform.stamp_ = time; + tf_transform.stamp = time; // loop over all joints for (map<string, double>::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){ std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jnt->first); if (seg != segments_.end()){ - tf::transformKDLToTF(seg->second.segment.pose(jnt->second), tf_transform); - tf_transform.frame_id_ = seg->second.root; - tf_transform.child_frame_id_ = seg->second.tip; + transformKDLToTF(seg->second.segment.pose(jnt->second), tf_transform); + tf_transform.frame_id = seg->second.root; + tf_transform.child_frame_id = seg->second.tip; tf_transforms.push_back(tf_transform); } } - tf_broadcaster_.sendTransform(tf_transforms); + for (std::vector<tf::StampedTransform>::const_iterator it = tf_transforms.begin(); + it != tf_transforms.end(); it++) { + tf_publisher->send_transform(*it); + } } // publish fixed transforms void RobotStatePublisherThread::publishFixedTransforms() { - ROS_DEBUG("Publishing transforms for moving joints"); + logger->log_debug(name(), "Publishing transforms for moving joints"); std::vector<tf::StampedTransform> tf_transforms; tf::StampedTransform tf_transform; - tf_transform.stamp_ = ros::Time::now()+ros::Duration(0.5); // future publish by 0.5 seconds + fawkes::Time now(clock); + tf_transform.stamp = now + 0.5; // future publish by 0.5 seconds // loop over all fixed segments for (map<string, SegmentPair>::const_iterator seg=segments_fixed_.begin(); seg != segments_fixed_.end(); seg++){ - tf::transformKDLToTF(seg->second.segment.pose(0), tf_transform); - tf_transform.frame_id_ = seg->second.root; - tf_transform.child_frame_id_ = seg->second.tip; + transformKDLToTF(seg->second.segment.pose(0), tf_transform); + tf_transform.frame_id = seg->second.root; + tf_transform.child_frame_id = seg->second.tip; tf_transforms.push_back(tf_transform); } - tf_broadcaster_.sendTransform(tf_transforms); + for (std::vector<tf::StampedTransform>::const_iterator it = tf_transforms.begin(); + it != tf_transforms.end(); it++) { + tf_publisher->send_transform(*it); + } } + +void RobotStatePublisherThread::transformKDLToTF(const KDL::Frame &k, fawkes::tf::Transform &t) + { + t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2])); + t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2], + k.M.data[3], k.M.data[4], k.M.data[5], + k.M.data[6], k.M.data[7], k.M.data[8])); + } diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.h b/src/plugins/robot_state_publisher/robot_state_publisher_thread.h index 8981eef..3cb9352 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.h +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.h @@ -68,6 +68,7 @@ #include <sensor_msgs/JointState.h> #include <ros/ros.h> #include <boost/scoped_ptr.hpp> +#include <kdl/kdl.hpp> #include <kdl/frames.hpp> #include <kdl/segment.hpp> #include <kdl/tree.hpp> @@ -102,10 +103,11 @@ private: * \param joint_positions A map of joint names and joint positions. * \param time The time at which the joint positions were recorded */ - void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time); + void publishTransforms(const std::map<std::string, double>& joint_positions, const fawkes::Time& time); void publishFixedTransforms(); void addChildren(const KDL::SegmentMap::const_iterator segment); + void transformKDLToTF(const KDL::Frame &k, fawkes::tf::Transform &t); std::map<std::string, SegmentPair> segments_, segments_fixed_; - *commit* fef8305f7c862eeeccf4fa309caa26a597574f01 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Mon Sep 9 14:43:30 2013 +0200 Subject: robot-publisher: publish information cfg/conf.d/robot_state_publisher.yaml | 7 + src/plugins/robot_state_publisher/Makefile | 3 +- .../robot_state_publisher_thread.cpp | 139 ++++++++++++++++++-- .../robot_state_publisher_thread.h | 34 ++++- 4 files changed, 162 insertions(+), 21 deletions(-) _Diff for modified files_: diff --git a/src/plugins/robot_state_publisher/Makefile b/src/plugins/robot_state_publisher/Makefile index 8d0be72..d6a9b5e 100644 --- a/src/plugins/robot_state_publisher/Makefile +++ b/src/plugins/robot_state_publisher/Makefile @@ -20,7 +20,8 @@ include $(BUILDSYSDIR)/ros.mk include $(BUILDSYSDIR)/boost.mk include $(BUILDCONFDIR)/tf/tf.mk -LIBS_robot_state_publisher = fawkescore fawkesutils fawkesaspects fawkesrosaspect fawkestf +LIBS_robot_state_publisher = fawkescore fawkesutils fawkesaspects fawkesrosaspect fawkestf \ + fawkesblackboard fawkesinterface JointInterface OBJS_robot_state_publisher = robot_state_publisher_plugin.o robot_state_publisher_thread.o OBJS_all = $(OBJS_robot_state_publisher) diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp index 8748136..8461aba 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp @@ -57,6 +57,12 @@ #include "robot_state_publisher_thread.h" #include <kdl/frames_io.hpp> +#include<kdl_parser/kdl_parser.hpp> + +#include <fstream> +#include <list> + +#define CFG_PREFIX "/robot_state_publisher/" using namespace fawkes; using namespace std; @@ -68,32 +74,72 @@ using namespace std; /** Constructor. */ RobotStatePublisherThread::RobotStatePublisherThread() -: Thread("RobotPublisherThread", Thread::OPMODE_WAITFORWAKEUP), - BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE) +: Thread("RobotStatePublisherThread", Thread::OPMODE_WAITFORWAKEUP), + BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE), // TODO check hook + TransformAspect(TransformAspect::ONLY_PUBLISHER, "robot_state_transforms"), + BlackBoardInterfaceListener("RobotStatePublisher") { - } void RobotStatePublisherThread::init() { + cfg_urdf_path_ = config->get_string(CFG_PREFIX"urdf_file"); -} + string urdf; + string line; + ifstream urdf_file(cfg_urdf_path_); + if (!urdf_file.is_open()) { + logger->log_error(name(), "failed to open URDF File %s", cfg_urdf_path_.c_str()); + throw; + } + while ( getline(urdf_file, line)) { + urdf += line; + } + urdf_file.close(); -void RobotStatePublisherThread::finalize() -{ + if (!kdl_parser::treeFromString(urdf, tree_)) { + logger->log_error(name(), "failed to parse urdf description to tree"); + throw; + } + // walk the tree and add segments to segments_ + addChildren(tree_.getRootSegment()); -} + // publish robot model to ROS + ros::param::set("/robot_description", urdf); -void RobotStatePublisherThread::loop() -{ + // check for open JointInterfaces + std::list<fawkes::JointInterface *> ifs = blackboard->open_multiple_for_reading<JointInterface>(); + for (std::list<JointInterface *>::iterator it = ifs.begin(); it != ifs.end(); it++) { + if (jointIsInModel((*it)->id())) { + ifs_.push_back(*it); + bbil_add_data_interface(*it); + } + else { + blackboard->close(*it); + } + } + // watch for creation of new JointInterfaces + bbio_add_observed_create("JointInterface"); + // watch for destruction of JointInterfaces + bbio_add_observed_destroy("JointInterface"); + // register to blackboard + blackboard->register_listener(this); + blackboard->register_observer(this); } +void RobotStatePublisherThread::finalize() +{ + blackboard->unregister_listener(this); + blackboard->unregister_observer(this); + for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); it++) { + blackboard->close(*it); + } +} -RobotStatePublisherThread::RobotStatePublisherThread() +void RobotStatePublisherThread::loop() { - // walk the tree and add segments to segments_ - addChildren(tree_.getRootSegment()); + publishFixedTransforms(); } @@ -122,7 +168,6 @@ void RobotStatePublisherThread::addChildren(const KDL::SegmentMap::const_iterato // publish moving transforms void RobotStatePublisherThread::publishTransforms(const map<string, double>& joint_positions, const Time& time) { - logger->log_debug(name(), "Publishing transforms for moving joints"); std::vector<tf::StampedTransform> tf_transforms; tf::StampedTransform tf_transform; tf_transform.stamp = time; @@ -147,7 +192,6 @@ void RobotStatePublisherThread::publishTransforms(const map<string, double>& joi // publish fixed transforms void RobotStatePublisherThread::publishFixedTransforms() { - logger->log_debug(name(), "Publishing transforms for moving joints"); std::vector<tf::StampedTransform> tf_transforms; tf::StampedTransform tf_transform; fawkes::Time now(clock); @@ -173,3 +217,70 @@ void RobotStatePublisherThread::transformKDLToTF(const KDL::Frame &k, fawkes::tf k.M.data[3], k.M.data[4], k.M.data[5], k.M.data[6], k.M.data[7], k.M.data[8])); } + + +/** + * @return true if the joint (represented by the interface) is part of our robot model + */ +bool RobotStatePublisherThread::jointIsInModel(const char *id) { + if (segments_.find(id) == segments_.end()) + return false; + return true; +} + +// InterfaceObserver +void +RobotStatePublisherThread::bb_interface_created(const char *type, const char *id) throw() +{ + if (strncmp(type, "JointInterface", __INTERFACE_TYPE_SIZE) != 0) return; + if (!jointIsInModel(id)) return; + JointInterface *interface; + try { + interface = blackboard->open_for_reading<JointInterface>(id); + } catch (Exception &e) { + logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what()); + return; + } + try { + ifs_.push_back(interface); + bbil_add_data_interface(interface); + blackboard->update_listener(this); + } catch (Exception &e) { + blackboard->close(interface); + logger->log_warn(name(), "Failed to register for %s:%s: %s", type, id, e.what()); + return; + } +} + +void +RobotStatePublisherThread::bb_interface_destroyed(const char *type, const char *id) throw() +{ + if (strncmp(type, "JointInterface", __INTERFACE_TYPE_SIZE) != 0) return; + for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); it++) { + if ((*it)->id() == id) { + bbil_remove_data_interface(*it); + blackboard->update_listener(this); + blackboard->close(*it); + ifs_.erase(it); + break; + } + } +} + +// InterfaceListener +void +RobotStatePublisherThread::bb_interface_data_changed(fawkes::Interface *interface) throw() +{ + JointInterface *jiface = dynamic_cast<JointInterface *>(interface); + if (!jiface) return; + jiface->read(); + std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jiface->id()); + if (seg == segments_.end()) return; + tf::StampedTransform transform; + transform.stamp = fawkes::Time(clock); + transform.frame_id = seg->second.root; + transform.child_frame_id = seg->second.tip; + transformKDLToTF(seg->second.segment.pose(jiface->position()), transform); + tf_publisher->send_transform(transform); + +} diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.h b/src/plugins/robot_state_publisher/robot_state_publisher_thread.h index 3cb9352..d62022c 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.h +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.h @@ -55,24 +55,32 @@ *********************************************************************/ -#ifndef __PLUGINS_ROBOTPUBLISHER_ROBOTSTATEPUBLISHER_THREAD_H_ -#define __PLUGINS_ROBOTPUBLISHER_ROBOTSTATEPUBLISHER_THREAD_H_ +#ifndef __PLUGINS_ROBOTSTATEPUBLISHER_ROBOTSTATEPUBLISHER_THREAD_H_ +#define __PLUGINS_ROBOTSTATEPUBLISHER_ROBOTSTATEPUBLISHER_THREAD_H_ #include <core/threading/thread.h> #include <aspect/logging.h> #include <aspect/blocked_timing.h> #include <aspect/clock.h> #include <aspect/tf.h> +#include <aspect/configurable.h> #include <plugins/ros/aspect/ros.h> +#include <aspect/blackboard.h> +#include <blackboard/interface_listener.h> +#include <blackboard/interface_observer.h> + +#include <interfaces/JointInterface.h> #include <sensor_msgs/JointState.h> #include <ros/ros.h> -#include <boost/scoped_ptr.hpp> #include <kdl/kdl.hpp> #include <kdl/frames.hpp> #include <kdl/segment.hpp> #include <kdl/tree.hpp> +#include <map> +#include <list> + class SegmentPair { public: @@ -89,7 +97,11 @@ class RobotStatePublisherThread public fawkes::BlockedTimingAspect, public fawkes::ClockAspect, public fawkes::TransformAspect, - public fawkes::ROSAspect + public fawkes::ConfigurableAspect, + public fawkes::ROSAspect, + public fawkes::BlackBoardAspect, + public fawkes::BlackBoardInterfaceObserver, + public fawkes::BlackBoardInterfaceListener { public: RobotStatePublisherThread(); @@ -98,6 +110,13 @@ public: virtual void loop(); virtual void finalize(); + // InterfaceObserver + virtual void bb_interface_created(const char *type, const char *id) throw(); + virtual void bb_interface_destroyed(const char *type, const char *id) throw(); + + // InterfaceListener + virtual void bb_interface_data_changed(fawkes::Interface *interface) throw(); + private: /** Publish transforms to tf * \param joint_positions A map of joint names and joint positions. @@ -108,11 +127,14 @@ private: void addChildren(const KDL::SegmentMap::const_iterator segment); void transformKDLToTF(const KDL::Frame &k, fawkes::tf::Transform &t); + bool jointIsInModel(const char *id); std::map<std::string, SegmentPair> segments_, segments_fixed_; - fawkes::tf::TransformPublisher tf_broadcaster_; - KDL::Tree& tree_; + KDL::Tree tree_; + std::string cfg_urdf_path_; + + std::list<fawkes::JointInterface *> ifs_; }; #endif - *commit* f5c36dcad2046cbec9948e611b8ec3c61e61fa23 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Fri Sep 27 19:46:55 2013 +0200 Subject: robot-publisher: warn if joint information is incomplete .../robot_state_publisher_thread.cpp | 10 ++++++++++ 1 files changed, 10 insertions(+), 0 deletions(-) _Diff for modified files_: diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp index 8461aba..d08e14c 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp @@ -107,10 +107,14 @@ void RobotStatePublisherThread::init() // publish robot model to ROS ros::param::set("/robot_description", urdf); + std::map<std::string, SegmentPair> unknown_segments = segments_; + // check for open JointInterfaces std::list<fawkes::JointInterface *> ifs = blackboard->open_multiple_for_reading<JointInterface>(); for (std::list<JointInterface *>::iterator it = ifs.begin(); it != ifs.end(); it++) { if (jointIsInModel((*it)->id())) { + logger->log_debug(name(), "Found joint information for %s", (*it)->id()); + unknown_segments.erase((*it)->id()); ifs_.push_back(*it); bbil_add_data_interface(*it); } @@ -118,6 +122,10 @@ void RobotStatePublisherThread::init() blackboard->close(*it); } } + for (map<string, SegmentPair>::const_iterator it = unknown_segments.begin(); + it != unknown_segments.end(); it++) { + logger->log_warn(name(), "No information for joint %s available", it->first.c_str()); + } // watch for creation of new JointInterfaces bbio_add_observed_create("JointInterface"); // watch for destruction of JointInterfaces @@ -241,6 +249,7 @@ RobotStatePublisherThread::bb_interface_created(const char *type, const char *id logger->log_warn(name(), "Failed to open %s:%s: %s", type, id, e.what()); return; } + logger->log_debug(name(), "Found joint information for %s", interface->id()); try { ifs_.push_back(interface); bbil_add_data_interface(interface); @@ -258,6 +267,7 @@ RobotStatePublisherThread::bb_interface_destroyed(const char *type, const char * if (strncmp(type, "JointInterface", __INTERFACE_TYPE_SIZE) != 0) return; for (std::list<JointInterface *>::iterator it = ifs_.begin(); it != ifs_.end(); it++) { if ((*it)->id() == id) { + logger->log_warn(name(), "JointInterface %s removed, but part of robot model", id); bbil_remove_data_interface(*it); blackboard->update_listener(this); blackboard->close(*it); - *commit* cfb250f37ff4d440371c52864f56783d0164407a - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Fri Sep 27 19:55:24 2013 +0200 Subject: robot-publisher: change timing hook to sensor acquire hook .../robot_state_publisher_thread.cpp | 2 +- 1 files changed, 1 insertions(+), 1 deletions(-) _Diff for modified files_: diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp index d08e14c..c85320e 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp @@ -75,7 +75,7 @@ using namespace std; /** Constructor. */ RobotStatePublisherThread::RobotStatePublisherThread() : Thread("RobotStatePublisherThread", Thread::OPMODE_WAITFORWAKEUP), - BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_WORLDSTATE), // TODO check hook + BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_ACQUIRE), TransformAspect(TransformAspect::ONLY_PUBLISHER, "robot_state_transforms"), BlackBoardInterfaceListener("RobotStatePublisher") { - *commit* 38985f94d335481ce20bb08206c0d1f7b9477a0d - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Fri Sep 27 20:33:39 2013 +0200 Subject: robot-publisher: remove unused function publishTransforms .../robot_state_publisher_thread.cpp | 25 -------------------- .../robot_state_publisher_thread.h | 6 +---- 2 files changed, 1 insertions(+), 30 deletions(-) _Diff for modified files_: diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp index c85320e..f26eef8 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp @@ -172,31 +172,6 @@ void RobotStatePublisherThread::addChildren(const KDL::SegmentMap::const_iterato } } - -// publish moving transforms -void RobotStatePublisherThread::publishTransforms(const map<string, double>& joint_positions, const Time& time) -{ - std::vector<tf::StampedTransform> tf_transforms; - tf::StampedTransform tf_transform; - tf_transform.stamp = time; - - // loop over all joints - for (map<string, double>::const_iterator jnt=joint_positions.begin(); jnt != joint_positions.end(); jnt++){ - std::map<std::string, SegmentPair>::const_iterator seg = segments_.find(jnt->first); - if (seg != segments_.end()){ - transformKDLToTF(seg->second.segment.pose(jnt->second), tf_transform); - tf_transform.frame_id = seg->second.root; - tf_transform.child_frame_id = seg->second.tip; - tf_transforms.push_back(tf_transform); - } - } - for (std::vector<tf::StampedTransform>::const_iterator it = tf_transforms.begin(); - it != tf_transforms.end(); it++) { - tf_publisher->send_transform(*it); - } -} - - // publish fixed transforms void RobotStatePublisherThread::publishFixedTransforms() { diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.h b/src/plugins/robot_state_publisher/robot_state_publisher_thread.h index d62022c..5776097 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.h +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.h @@ -118,11 +118,7 @@ public: virtual void bb_interface_data_changed(fawkes::Interface *interface) throw(); private: - /** Publish transforms to tf - * \param joint_positions A map of joint names and joint positions. - * \param time The time at which the joint positions were recorded - */ - void publishTransforms(const std::map<std::string, double>& joint_positions, const fawkes::Time& time); + void publishFixedTransforms(); void addChildren(const KDL::SegmentMap::const_iterator segment); - *commit* 887ca9a31bf7a0ee6df2116d473b01a1b493728b - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Tue Feb 18 18:34:27 2014 +0100 Subject: robot-publisher: adapt to use fawkes' KDL Parser instead of ROS Package src/plugins/robot_state_publisher/Makefile | 56 ++++++-------------- .../robot_state_publisher_thread.cpp | 5 +-- .../robot_state_publisher_thread.h | 4 -- 3 files changed, 18 insertions(+), 47 deletions(-) _Diff for modified files_: diff --git a/src/plugins/robot_state_publisher/Makefile b/src/plugins/robot_state_publisher/Makefile index d6a9b5e..353d149 100644 --- a/src/plugins/robot_state_publisher/Makefile +++ b/src/plugins/robot_state_publisher/Makefile @@ -19,58 +19,36 @@ include $(BASEDIR)/etc/buildsys/config.mk include $(BUILDSYSDIR)/ros.mk include $(BUILDSYSDIR)/boost.mk include $(BUILDCONFDIR)/tf/tf.mk +include $(BUILDCONFDIR)/kdl_parser/kdl_parser.mk LIBS_robot_state_publisher = fawkescore fawkesutils fawkesaspects fawkesrosaspect fawkestf \ - fawkesblackboard fawkesinterface JointInterface + fawkesblackboard fawkesinterface JointInterface fawkeskdl_parser OBJS_robot_state_publisher = robot_state_publisher_plugin.o robot_state_publisher_thread.o OBJS_all = $(OBJS_robot_state_publisher) -ifeq ($(HAVE_ROS),1) - ifeq ($(HAVE_TF),1) - ifeq ($(call ros-have-pkg,sensor_msgs),1) - ifeq ($(call ros-have-pkg,kdl),1) - ifeq ($(call ros-have-pkg,kdl_parser),1) - PLUGINS_all = $(PLUGINDIR)/robot_state_publisher.so - CFLAGS += $(CFLAGS_ROS) $(call ros-pkg-cflags,sensor_msgs) \ - $(call ros-pkg-cflags,kdl) \ - $(call ros-pkg-cflags,kdl_parser) \ - $(call boost-lib-cflags,scoped_ptr) \ - $(CFLAGS_TF) - LDFLAGS += $(LDFLAGS_ROS) $(call ros-pkg-lflags,sensor_msgs) \ - $(call ros-pkg-lflags,kdl) \ - $(call ros-pkg-lflags,kdl_parser) \ - $(call boost-lib-lflags,scoped_ptr) \ - $(LDFLAGS_TF) - else - WARN_TARGETS += warning_kdl_parser - endif - else - WARN_TARGETS += warning_kdl - endif - else - WARN_TARGETS += warning_sensor_msgs - endif - else - WARN_TARGETS += warning_tf - endif +HAVE_KDL = $(if $(shell $(PKGCONFIG) --exists 'orocos-kdl'; echo $${?/1/}),1,0) + +ifeq ($(HAVE_KDLPARSER),1) + ifeq ($(HAVE_TF),1) + PLUGINS_all = $(PLUGINDIR)/robot_state_publisher.so + CFLAGS += $(CFLAGS_TF) $(CFLAGS_KDLPARSER) + LDFLAGS += $(LDFLAGS_TF) $(LDFLAGS_KDLPARSER) \ + $(call boost-lib-lflags,scoped_ptr) + else + WARN_TARGETS += warning_tf + endif else - WARN_TARGETS += warning_ros + WARN_TARGETS += warning_kdlparser endif ifeq ($(OBJSSUBMAKE),1) all: $(WARN_TARGETS) -.PHONY: warning_ros warning_sensor_msgs warning_tf warning_kdl_parser warning_kdl -warning_ros: - $(SILENT)echo -e "$(INDENT_PRINT) --> $(TRED)Disabling robot state publisher plugin$(TNORMAL) (ROS not available)" +.PHONY: warning_tf warning_kdlparser warning_tf: $(SILENT)echo -e "$(INDENT_PRINT)--> $(TRED)Disabling robot state publisher plugin$(TNORMAL) (tf not available)" -warning_sensor_msgs: - $(SILENT)echo -e "$(INDENT_PRINT) --> $(TRED)Disabling robot state publisher plugin$(TNORMAL) (ROS package sensor_msgs not installed)" -warning_kdl_parser: - $(SILENT)echo -e "$(INDENT_PRINT) --> $(TRED)Disabling robot state publisher plugin$(TNORMAL) (ROS package kdl_parser not installed)" -warning_kdl: - $(SILENT)echo -e "$(INDENT_PRINT) --> $(TRED)Disabling robot state publisher plugin$(TNORMAL) (ROS package kdl not installed)" +warning_kdlparser: + $(SILENT)echo -e "$(INDENT_PRINT) --> $(TRED)Disabling robot state publisher plugin$(TNORMAL) (kdl_parser not available)" endif include $(BUILDSYSDIR)/base.mk diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp index f26eef8..aa411e9 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.cpp @@ -57,7 +57,7 @@ #include "robot_state_publisher_thread.h" #include <kdl/frames_io.hpp> -#include<kdl_parser/kdl_parser.hpp> +#include <kdl_parser/kdl_parser.h> #include <fstream> #include <list> @@ -104,9 +104,6 @@ void RobotStatePublisherThread::init() // walk the tree and add segments to segments_ addChildren(tree_.getRootSegment()); - // publish robot model to ROS - ros::param::set("/robot_description", urdf); - std::map<std::string, SegmentPair> unknown_segments = segments_; // check for open JointInterfaces diff --git a/src/plugins/robot_state_publisher/robot_state_publisher_thread.h b/src/plugins/robot_state_publisher/robot_state_publisher_thread.h index 5776097..25387fd 100644 --- a/src/plugins/robot_state_publisher/robot_state_publisher_thread.h +++ b/src/plugins/robot_state_publisher/robot_state_publisher_thread.h @@ -64,15 +64,12 @@ #include <aspect/clock.h> #include <aspect/tf.h> #include <aspect/configurable.h> -#include <plugins/ros/aspect/ros.h> #include <aspect/blackboard.h> #include <blackboard/interface_listener.h> #include <blackboard/interface_observer.h> #include <interfaces/JointInterface.h> -#include <sensor_msgs/JointState.h> -#include <ros/ros.h> #include <kdl/kdl.hpp> #include <kdl/frames.hpp> #include <kdl/segment.hpp> @@ -98,7 +95,6 @@ class RobotStatePublisherThread public fawkes::ClockAspect, public fawkes::TransformAspect, public fawkes::ConfigurableAspect, - public fawkes::ROSAspect, public fawkes::BlackBoardAspect, public fawkes::BlackBoardInterfaceObserver, public fawkes::BlackBoardInterfaceListener -- Fawkes Robotics Framework http://www.fawkesrobotics.org _______________________________________________ fawkes-commits mailing list fawkes-commits@lists.kbsg.rwth-aachen.de https://lists.kbsg.rwth-aachen.de/listinfo/fawkes-commits
