From 67fd8ac9545b856afe668d1bf7eb246068fb3c3b Mon Sep 17 00:00:00 2001 From: akgoins Date: Thu, 24 Mar 2016 10:05:58 -0500 Subject: [PATCH 1/4] Added class for publishing TF frames --- urdf_editor/CMakeLists.txt | 3 + .../include/urdf_editor/joint_property.h | 4 + urdf_editor/include/urdf_editor/my_rviz.h | 1 + .../include/urdf_editor/urdf_transforms.h | 50 ++++++++ urdf_editor/package.xml | 5 + urdf_editor/src/joint_property.cpp | 10 ++ urdf_editor/src/my_rviz.cpp | 7 ++ urdf_editor/src/urdf_transforms.cpp | 109 ++++++++++++++++++ 8 files changed, 189 insertions(+) create mode 100644 urdf_editor/include/urdf_editor/urdf_transforms.h create mode 100644 urdf_editor/src/urdf_transforms.cpp diff --git a/urdf_editor/CMakeLists.txt b/urdf_editor/CMakeLists.txt index a419c7f..a28531b 100644 --- a/urdf_editor/CMakeLists.txt +++ b/urdf_editor/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) include(${QT_USE_FILE}) find_package(urdfdom REQUIRED) +find_package(Boost REQUIRED) set(editor_SRCS src/main.cpp @@ -15,6 +16,7 @@ set(editor_SRCS src/link_property.cpp src/urdf_property.cpp src/my_rviz.cpp + src/urdf_transforms.cpp ) set(editor_HDRS @@ -23,6 +25,7 @@ set(editor_HDRS include/urdf_editor/link_property.h include/urdf_editor/urdf_property.h include/urdf_editor/my_rviz.h + include/urdf_editor/urdf_transforms.h ) set(editor_UIS diff --git a/urdf_editor/include/urdf_editor/joint_property.h b/urdf_editor/include/urdf_editor/joint_property.h index c1d8633..bef9d1a 100644 --- a/urdf_editor/include/urdf_editor/joint_property.h +++ b/urdf_editor/include/urdf_editor/joint_property.h @@ -6,6 +6,7 @@ #include #include "urdf_editor/common.h" #include +#include namespace urdf_editor { @@ -35,6 +36,7 @@ namespace urdf_editor QtProperty *top_item_; bool loading_; + }; class JointAxisProperty : public QObject @@ -245,6 +247,8 @@ namespace urdf_editor QtVariantProperty *parent_item_; QtVariantProperty *child_item_; + URDFTransformer tf_transformer_; + }; typedef boost::shared_ptr JointPropertyPtr; } diff --git a/urdf_editor/include/urdf_editor/my_rviz.h b/urdf_editor/include/urdf_editor/my_rviz.h index ccc29e8..2a82c5e 100644 --- a/urdf_editor/include/urdf_editor/my_rviz.h +++ b/urdf_editor/include/urdf_editor/my_rviz.h @@ -38,6 +38,7 @@ namespace urdf_editor rviz::RenderPanel *render_panel_; moveit_rviz_plugin::RobotStateDisplay *robot_display_; rviz::Display *grid_display_; + rviz::Display *tf_display_; ros::NodeHandle nh_; }; diff --git a/urdf_editor/include/urdf_editor/urdf_transforms.h b/urdf_editor/include/urdf_editor/urdf_transforms.h new file mode 100644 index 0000000..a701789 --- /dev/null +++ b/urdf_editor/include/urdf_editor/urdf_transforms.h @@ -0,0 +1,50 @@ +#ifndef URDF_TRANSFORMS_H +#define URDF_TRANSFORMS_H + +#include +#include +#include +#include + + +#include +#include +#include + + + +class URDFTransformer{ + +public: + + URDFTransformer(); + ~URDFTransformer(); + + void updateTransforms(); + + void addLink(std::string parent, std::string child); + + void removeLink(std::string name); + + void updateLink(std::string parent, std::string child); + + void updateLink(std::string parent, geometry_msgs::Quaternion quat); + + void updateLink(std::string parent, geometry_msgs::Vector3 origin); + + void updateLink(std::string parent, geometry_msgs::Transform trans); + +private: + + tf::tfMessage frames_; + + boost::thread *worker_; + boost::mutex data_lock_; + + tf::TransformBroadcaster broadcaster_; + + void worker_thread(); + +}; + +#endif // URDF_TRANSFORMS_H diff --git a/urdf_editor/package.xml b/urdf_editor/package.xml index 080d077..4c13c01 100644 --- a/urdf_editor/package.xml +++ b/urdf_editor/package.xml @@ -16,9 +16,14 @@ rviz qt_property_browser moveit_ros_visualization + geometry_msgs + tf roscpp rviz qt_property_browser moveit_ros_visualization + geometry_msgs + tf + diff --git a/urdf_editor/src/joint_property.cpp b/urdf_editor/src/joint_property.cpp index 385a02a..d48d1a7 100644 --- a/urdf_editor/src/joint_property.cpp +++ b/urdf_editor/src/joint_property.cpp @@ -638,6 +638,16 @@ namespace urdf_editor this, SLOT(onChildValueChanged(QtProperty *, const QVariant &))); } + tf_transformer_.updateLink(joint_->parent_link_name, joint_->child_link_name); + geometry_msgs::Vector3 vect; + vect.x = joint_->parent_to_joint_origin_transform.position.x; + vect.y = joint_->parent_to_joint_origin_transform.position.y; + vect.z = joint_->parent_to_joint_origin_transform.position.z; + tf_transformer_.updateLink(joint_->parent_link_name, vect); + geometry_msgs::Quaternion quat; + joint_->parent_to_joint_origin_transform.rotation.getQuaternion(quat.x, quat.y, quat.z, quat.w); + tf_transformer_.updateLink(joint_->parent_link_name, quat); + loading_ = false; } diff --git a/urdf_editor/src/my_rviz.cpp b/urdf_editor/src/my_rviz.cpp index 0603a26..a4a0d3b 100644 --- a/urdf_editor/src/my_rviz.cpp +++ b/urdf_editor/src/my_rviz.cpp @@ -20,6 +20,8 @@ namespace urdf_editor render_panel_->setEnabled(true); render_panel_->setBackgroundColor(Ogre::ColourValue(0.25, 0.25, 0.25, 1)); + manager_->setFixedFrame("base_link"); + robot_display_ = new moveit_rviz_plugin::RobotStateDisplay(); robot_display_->setName("RobotModel"); robot_display_->subProp("Robot Description")->setValue("ros_workbench"); @@ -27,6 +29,11 @@ namespace urdf_editor grid_display_ = manager_->createDisplay("rviz/Grid", "MyGrid", true); grid_display_->subProp("Line Style")->setValue("Billboards"); + + tf_display_ = manager_->createDisplay("rviz/TF", "TF", true); + tf_display_->subProp("Show Arrows")->setValue("true"); + tf_display_->subProp("Show Names")->setValue("true"); + tf_display_->subProp("Show Axes")->setValue("true"); } // Destructor diff --git a/urdf_editor/src/urdf_transforms.cpp b/urdf_editor/src/urdf_transforms.cpp new file mode 100644 index 0000000..6ab6ddd --- /dev/null +++ b/urdf_editor/src/urdf_transforms.cpp @@ -0,0 +1,109 @@ +#include + + + +URDFTransformer::URDFTransformer() +{ + worker_ = new boost::thread(boost::bind(&URDFTransformer::worker_thread, this)); +} + +URDFTransformer::~URDFTransformer() +{ + delete worker_; +} + +void URDFTransformer::worker_thread() +{ + ros::Rate rt = 10; + while(ros::ok()) + { + tf::tfMessage tf_copy; + { + boost::lock_guard lock(data_lock_); + tf_copy = frames_; + } + + if(tf_copy.transforms.size() > 0) + { + broadcaster_.sendTransform(tf_copy.transforms); + } + rt.sleep(); + } +} + +void URDFTransformer::addLink(std::string parent, std::string child) +{ + boost::lock_guard lock(data_lock_); + for(int i = 0; i < frames_.transforms.size(); ++i) + { + if(parent.compare(frames_.transforms[i].header.frame_id) == 0) + { + ROS_WARN("frame name %s already exists, not adding to list", parent.c_str()); + return; + } + } + + geometry_msgs::TransformStamped new_frame; + new_frame.header.frame_id = parent; + new_frame.child_frame_id = child; + frames_.transforms.push_back(new_frame); +} + +void URDFTransformer::updateLink(std::string parent, std::string child) +{ + boost::lock_guard lock(data_lock_); + int i; + for(i = 0; i < frames_.transforms.size(); ++i) + { + // find frame to update + if(parent.compare(frames_.transforms[i].header.frame_id) == 0) + { + frames_.transforms[i].child_frame_id = child; + break; + } + } + + // Do we need to add the transform here or just return a bool success/failure? + if(i == frames_.transforms.size()) + { + geometry_msgs::TransformStamped new_frame; + new_frame.header.frame_id = parent; + new_frame.child_frame_id = child; + frames_.transforms.push_back(new_frame); + } +} + +void URDFTransformer::updateLink(std::string parent, geometry_msgs::Transform trans) +{ + updateLink(parent, trans.rotation); + updateLink(parent, trans.translation); +} + +void URDFTransformer::updateLink(std::string parent, geometry_msgs::Quaternion quat) +{ + boost::lock_guard lock(data_lock_); + for(int i = 0; i < frames_.transforms.size(); ++i) + { + // find frame to update + if(parent.compare(frames_.transforms[i].header.frame_id) == 0) + { + frames_.transforms[i].transform.rotation = quat; + break; + } + } +} + +void URDFTransformer::updateLink(std::string parent, geometry_msgs::Vector3 origin) +{ + boost::lock_guard lock(data_lock_); + for(int i = 0; i < frames_.transforms.size(); ++i) + { + // find frame to update + if(parent.compare(frames_.transforms[i].header.frame_id) == 0) + { + frames_.transforms[i].transform.translation = origin; + break; + } + } +} + From 395a175eda235b5f0ee4e273b9578c34c8bfe367 Mon Sep 17 00:00:00 2001 From: akgoins Date: Fri, 1 Apr 2016 10:26:03 -0500 Subject: [PATCH 2/4] Added function to remove link and added documentation --- .../include/urdf_editor/urdf_transforms.h | 43 ++++++++++++++++--- urdf_editor/src/urdf_transforms.cpp | 15 ++++++- 2 files changed, 50 insertions(+), 8 deletions(-) diff --git a/urdf_editor/include/urdf_editor/urdf_transforms.h b/urdf_editor/include/urdf_editor/urdf_transforms.h index a701789..d88a613 100644 --- a/urdf_editor/include/urdf_editor/urdf_transforms.h +++ b/urdf_editor/include/urdf_editor/urdf_transforms.h @@ -12,7 +12,8 @@ #include - +namespace urdf_editor +{ class URDFTransformer{ public: @@ -20,31 +21,59 @@ class URDFTransformer{ URDFTransformer(); ~URDFTransformer(); - void updateTransforms(); - + /** + * @brief addLink Adds a link to the transfrom list to be published + * @param parent The name of the parent frame id for the link + * @param child The name of the child frame id for the link + */ void addLink(std::string parent, std::string child); + /** + * @brief removeLink Removes a link from the list if it exists + * @param name The name of the link to remove from the list + */ void removeLink(std::string name); + /** + * @brief updateLink Looks for a link in the existing tree to update the child information for + * @param parent The name of the link to find and update + * @param child The name of the child to update to + */ void updateLink(std::string parent, std::string child); + /** + * @brief updateLink Looks for a link in the existing tree to update the origin orientation information for + * @param parent The name of the link to find and update + * @param quat The orientation, in quaternion format + */ void updateLink(std::string parent, geometry_msgs::Quaternion quat); + /** + * @brief updateLink Looks for a link in the existing tree to update the origin position information for + * @param parent The name of the link to find and update + * @param origin The origin position + */ void updateLink(std::string parent, geometry_msgs::Vector3 origin); + /** + * @brief updateLink Looks for a link in the existing tree to update the origin information for + * @param parent The name of the link to find and update + * @param trans The origin information, in tranformation format + */ void updateLink(std::string parent, geometry_msgs::Transform trans); private: - tf::tfMessage frames_; + tf::tfMessage frames_; /**< @brief The list of frames to be published for visualization */ - boost::thread *worker_; - boost::mutex data_lock_; + boost::thread *worker_; /**< @brief The worker thread, created on construction, for publishing the TF frames */ + boost::mutex data_lock_; /**< @brief The mutex data lock for reading/writing the TF frames */ - tf::TransformBroadcaster broadcaster_; + tf::TransformBroadcaster broadcaster_; /**< @brief The TF broadcaster for publishing frames to the /tf ROS topic */ void worker_thread(); }; +} #endif // URDF_TRANSFORMS_H diff --git a/urdf_editor/src/urdf_transforms.cpp b/urdf_editor/src/urdf_transforms.cpp index 6ab6ddd..c9994c2 100644 --- a/urdf_editor/src/urdf_transforms.cpp +++ b/urdf_editor/src/urdf_transforms.cpp @@ -1,6 +1,6 @@ #include - +namespace urdf_editor { URDFTransformer::URDFTransformer() { @@ -49,6 +49,18 @@ void URDFTransformer::addLink(std::string parent, std::string child) frames_.transforms.push_back(new_frame); } +void URDFTransformer::removeLink(std::string name) +{ + boost::lock_guard lock(data_lock_); + for(int i = 0; i < frames_.transforms.size(); ++i) + { + if(name.compare(frames_.transforms[i].header.frame_id) == 0) + { + frames_.transforms.erase(frames_.transforms.begin()+i); + } + } +} + void URDFTransformer::updateLink(std::string parent, std::string child) { boost::lock_guard lock(data_lock_); @@ -107,3 +119,4 @@ void URDFTransformer::updateLink(std::string parent, geometry_msgs::Vector3 orig } } +} From 61e7f40b0e619d8d884381df3c65ae29292c0486 Mon Sep 17 00:00:00 2001 From: akgoins Date: Fri, 1 Apr 2016 10:34:41 -0500 Subject: [PATCH 3/4] Added function to update TF frames on joint property change --- .../include/urdf_editor/joint_property.h | 1 + urdf_editor/src/joint_property.cpp | 17 +++++++++++++++++ 2 files changed, 18 insertions(+) diff --git a/urdf_editor/include/urdf_editor/joint_property.h b/urdf_editor/include/urdf_editor/joint_property.h index 22ad468..18cf18f 100644 --- a/urdf_editor/include/urdf_editor/joint_property.h +++ b/urdf_editor/include/urdf_editor/joint_property.h @@ -310,6 +310,7 @@ namespace urdf_editor private slots: void onValueChanged(QtProperty *property, const QVariant &val); void onChildValueChanged(QtProperty *property, const QVariant &val); + void updateTF(QtProperty *property, const QVariant &val); signals: void jointNameChanged(JointProperty *property, const QVariant &val); diff --git a/urdf_editor/src/joint_property.cpp b/urdf_editor/src/joint_property.cpp index 6aa7073..cc02089 100644 --- a/urdf_editor/src/joint_property.cpp +++ b/urdf_editor/src/joint_property.cpp @@ -592,6 +592,8 @@ namespace urdf_editor origin_property_.reset(new OriginProperty(joint->parent_to_joint_origin_transform)); QObject::connect(origin_property_.get(), SIGNAL(valueChanged(QtProperty *, const QVariant &)), this, SLOT(onChildValueChanged(QtProperty *, const QVariant &))); + QObject::connect(origin_property_.get(), SIGNAL(valueChanged(QtProperty *, const QVariant &)), + this, SLOT(updateTF(QtProperty *, const QVariant &))); } p_norm = joint_->axis.x * joint_->axis.x; @@ -797,6 +799,21 @@ namespace urdf_editor emit JointProperty::valueChanged(); } + void JointProperty::updateTF(QtProperty *property, const QVariant &val) + { + tf_transformer_.updateLink(joint_->parent_link_name, joint_->child_link_name); + geometry_msgs::Vector3 vect; + vect.x = joint_->parent_to_joint_origin_transform.position.x; + vect.y = joint_->parent_to_joint_origin_transform.position.y; + vect.z = joint_->parent_to_joint_origin_transform.position.z; + tf_transformer_.updateLink(joint_->parent_link_name, vect); + ROS_INFO_STREAM("joint property changed: " << joint_->parent_link_name << " " << vect); + + geometry_msgs::Quaternion quat; + joint_->parent_to_joint_origin_transform.rotation.getQuaternion(quat.x, quat.y, quat.z, quat.w); + tf_transformer_.updateLink(joint_->parent_link_name, quat); + } + void JointProperty::onChildValueChanged(QtProperty *property, const QVariant &val) { if (loading_) From f916110a6d5bfe31ed33a9b14fb603ba0af6bfc9 Mon Sep 17 00:00:00 2001 From: Stephen Hart Date: Fri, 1 Apr 2016 12:52:28 -0500 Subject: [PATCH 4/4] added changes to have parent and child links for joints drop down menus --- urdf_editor/src/joint_property.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/urdf_editor/src/joint_property.cpp b/urdf_editor/src/joint_property.cpp index cc02089..6b79b5d 100644 --- a/urdf_editor/src/joint_property.cpp +++ b/urdf_editor/src/joint_property.cpp @@ -574,12 +574,13 @@ namespace urdf_editor type_item_ = manager_->addProperty(QtVariantPropertyManager::enumTypeId(), tr("Type")); type_item_->setAttribute(Common::attributeStr(EnumNames), QStringList() << tr("Unknown") << tr("Revolute") << tr("Continuous") << tr("Prismatic") << tr("Floating") << tr("Planar") << tr("Fixed")); - parent_item_ = manager_->addProperty(QVariant::String, tr("Parent")); - parent_item_->setAttribute(Common::attributeStr(ReadOnly), true); + parent_item_ = manager_->addProperty(QtVariantPropertyManager::enumTypeId(), tr("Parent")); + parent_item_->setAttribute(Common::attributeStr(EnumNames), link_names_); child_item_ = manager_->addProperty(QtVariantPropertyManager::enumTypeId(), tr("Child")); child_item_->setAttribute(Common::attributeStr(EnumNames), link_names_); + origin = joint_->parent_to_joint_origin_transform; p_norm = origin.position.x * origin.position.x; p_norm += origin.position.y * origin.position.y; @@ -665,10 +666,14 @@ namespace urdf_editor loading_ = true; name_item_->setValue(QString::fromStdString(joint_->name)); type_item_->setValue(joint_->type); - parent_item_->setValue(QString::fromStdString(joint_->parent_link_name)); + + parent_item_->setAttribute(Common::attributeStr(EnumNames), link_names_); + parent_item_->setValue(link_names_.indexOf(QString::fromStdString(joint_->parent_link_name))); + child_item_->setAttribute(Common::attributeStr(EnumNames), link_names_); child_item_->setValue(link_names_.indexOf(QString::fromStdString(joint_->child_link_name))); + if (origin_property_) origin_property_->loadData(); @@ -788,7 +793,7 @@ namespace urdf_editor } else if (name == "Parent") { - joint_->parent_link_name = val.toString().toStdString(); + joint_->parent_link_name = link_names_[val.toInt()].toStdString(); } else if (name == "Child") {