Add dropdowns for 'parent' and 'child' fields of 'joint' (issue 48 fix)

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

urdf_editor/include/urdf_editor/joint_property.h

@@ -6,6 +6,7 @@
 #include <qtvariantproperty.h>
 #include "urdf_editor/common.h"
 #include <urdf_model/joint.h>
+#include <urdf_editor/urdf_transforms.h>
 
 namespace urdf_editor
 {
@@ -35,6 +36,7 @@ namespace urdf_editor
     QtProperty *top_item_;
     bool loading_;
 
+
   };
 
   class JointAxisProperty : public QObject
@@ -308,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);
@@ -332,6 +335,8 @@ namespace urdf_editor
     QtVariantProperty *parent_item_;
     QtVariantProperty *child_item_;
 
+    URDFTransformer tf_transformer_;
+
   };
   typedef boost::shared_ptr<JointProperty> JointPropertyPtr;
 }

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_;
   };
 

urdf_editor/include/urdf_editor/urdf_transforms.h

@@ -0,0 +1,79 @@
+#ifndef URDF_TRANSFORMS_H
+#define URDF_TRANSFORMS_H
+
+#include <ros/ros.h>
+#include <tf/tfMessage.h>
+#include <tf/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
+
+
+#include <boost/thread/locks.hpp>
+#include <boost/thread/mutex.hpp>
+#include <boost/thread.hpp>
+
+
+namespace urdf_editor
+{
+class URDFTransformer{
+
+public:
+
+  URDFTransformer();
+  ~URDFTransformer();
+
+  /**
+   * @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_; /**< @brief The list of frames to be published for visualization */
+
+  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_; /**< @brief The TF broadcaster for publishing frames to the /tf ROS topic */
+
+  void worker_thread();
+
+};
+
+}
+#endif // URDF_TRANSFORMS_H

urdf_editor/package.xml

@@ -16,9 +16,14 @@
   <build_depend>rviz</build_depend>
   <build_depend>qt_property_browser</build_depend>
   <build_depend>moveit_ros_visualization</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>tf</build_depend>
 
   <run_depend>roscpp</run_depend>
   <run_depend>rviz</run_depend>
   <run_depend>qt_property_browser</run_depend>
   <run_depend>moveit_ros_visualization</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>tf</run_depend>
+
 </package>

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;
@@ -592,6 +593,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;
@@ -639,6 +642,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;
   }
 
@@ -653,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();
 
@@ -776,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")
     {
@@ -787,6 +804,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_)

urdf_editor/src/my_rviz.cpp

@@ -20,13 +20,20 @@ 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");
     manager_->addDisplay(robot_display_, false);
 
     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

urdf_editor/src/urdf_transforms.cpp

@@ -0,0 +1,122 @@
+#include <urdf_editor/urdf_transforms.h>
+
+namespace urdf_editor {
+
+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<boost::mutex> 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<boost::mutex> 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::removeLink(std::string name)
+{
+  boost::lock_guard<boost::mutex> 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<boost::mutex> 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<boost::mutex> 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<boost::mutex> 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;
+    }
+  }
+}
+
+}