Updated meta-data, JointStatePublisher with merging ability

easy_state_publisher/CMakeLists.txt

@@ -1,26 +0,0 @@
-cmake_minimum_required(VERSION 2.4.6)
-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-
-# Set the build type.  Options are:
-#  Coverage       : w/ debug symbols, w/o optimization, w/ code-coverage
-#  Debug          : w/ debug symbols, w/o optimization
-#  Release        : w/o debug symbols, w/ optimization
-#  RelWithDebInfo : w/ debug symbols, w/ optimization
-#  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
-rosbuild_init()
-
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#rosbuild_genmsg()
-#uncomment if you have defined services
-#rosbuild_gensrv()
-
-rosbuild_add_executable(easy_state_publisher src/easy_state_publisher.cpp)
-rosbuild_add_executable(easy_tf_publisher src/easy_tf_publisher.cpp)
-

easy_state_publisher/Makefile

@@ -1 +0,0 @@
-include $(shell rospack find mk)/cmake.mk

easy_state_publisher/mainpage.dox

@@ -1,26 +0,0 @@
-/**
-\mainpage
-\htmlinclude manifest.html
-
-\b easy_state_publisher is ... 
-
-<!-- 
-Provide an overview of your package.
--->
-
-
-\section codeapi Code API
-
-<!--
-Provide links to specific auto-generated API documentation within your
-package that is of particular interest to a reader. Doxygen will
-document pretty much every part of your code, so do your best here to
-point the reader to the actual API.
-
-If your codebase is fairly large or has different sets of APIs, you
-should use the doxygen 'group' tag to keep these APIs together. For
-example, the roscpp documentation has 'libros' group.
--->
-
-
-*/

easy_state_publisher/manifest.xml

@@ -1,16 +0,0 @@
-<package>
-  <description brief="Easy State Publisher publishes JointState and/or TF messages for a given robot model">
-	Easy State Publisher publishes JointState and/or TF messages for a given robot model
-  </description>
-  <author>David Lu!!</author>
-  <license>BSD</license>
-  <review status="unreviewed" notes=""/>
-  <url>http://ros.org/wiki/easy_state_publisher</url>
-  <depend package="roscpp"/>
-  <depend package="robot_state_publisher"/>
-  <depend package="urdf" />
-  <depend package="kdl_parser"/>
-  <depend package="sensor_msgs"/>
-</package>
-
-

easy_state_publisher/src/easy_state_publisher.cpp

@@ -1,58 +0,0 @@
-#include <string>
-#include <ros/ros.h>
-#include <sensor_msgs/JointState.h>
-#include <urdf/model.h>
-#include <urdf/joint.h>
-#include <map>
-
-int main(int argc, char** argv) {
-    ros::init(argc, argv, "easy_state_publisher");
-    ros::NodeHandle n;
-    ros::Publisher  m_joint_pub = n.advertise
-                                  <sensor_msgs::JointState>("joint_states", 1);
-    ros::Rate loop_rate(2);
-
-    urdf::Model my_model;
-    if (!my_model.initParam("/robot_description")) {
-        ROS_ERROR("Failed to parse urdf robot model");
-        return false;
-    }
-
-    typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > joint_map;
-    typedef std::map<std::string, double> position_map;
-    joint_map joints = my_model.joints_;
-    position_map positions;
-
-    int count = 0;
-    for (joint_map::const_iterator it =  joints.begin(); it !=  joints.end(); ++it) {
-        std::string name = it->first;
-        urdf::Joint* joint = it->second.get();
-        if (joint->type!=urdf::Joint::FIXED) {
-            positions[name] = 0;
-            count++;
-            ROS_INFO("%s", name.c_str());
-        }
-    }
-
-    sensor_msgs::JointState joint_state;
-    joint_state.name.resize(count);
-    joint_state.position.resize(count);
-    joint_state.velocity.resize(count);
-    int i = 0;
-    for (position_map::const_iterator it =  positions.begin(); it !=  positions.end(); ++it) {
-        joint_state.name[i++] = it->first.c_str();
-    }
-
-    while (ros::ok()) {
-
-        joint_state.header.stamp = ros::Time::now();
-        m_joint_pub.publish(joint_state);
-
-
-        // This will adjust as needed per iteration
-        loop_rate.sleep();
-    }
-
-
-    return 0;
-}

easy_state_publisher/src/easy_tf_publisher.cpp

@@ -1,50 +0,0 @@
-#include <string>
-#include <ros/ros.h>
-#include <robot_state_publisher/robot_state_publisher.h>
-#include <urdf/model.h>
-#include <urdf/joint.h>
-#include <kdl_parser/kdl_parser.hpp>
-#include <map>
-
-int main(int argc, char** argv) {
-    ros::init(argc, argv, "easy_state_publisher");
-    ros::NodeHandle n;
-    ros::Rate loop_rate(30);
-
-    urdf::Model my_model;
-    if (!my_model.initParam("/robot_description")) {
-        ROS_ERROR("Failed to parse urdf robot model");
-        return false;
-    }
-    KDL::Tree my_tree;
-    if (!kdl_parser::treeFromUrdfModel(my_model, my_tree)) {
-        ROS_ERROR("Failed to construct kdl tree");
-        return false;
-    }
-    robot_state_publisher::RobotStatePublisher* publisher = new robot_state_publisher::RobotStatePublisher(my_tree);
-
-    typedef std::map<std::string, boost::shared_ptr<urdf::Joint> > joint_map;
-
-    std::map<std::string, double> positions;
-    joint_map joints = my_model.joints_;
-
-    for (joint_map::const_iterator it =  joints.begin(); it !=  joints.end(); ++it) {
-        std::string name = it->first;
-        urdf::Joint* joint = it->second.get();
-        if (joint->type==urdf::Joint::REVOLUTE || joint->type==urdf::Joint::CONTINUOUS ||
-                joint->type==urdf::Joint::PRISMATIC)
-            positions[name] = 0;
-    }
-    while (ros::ok()) {
-        publisher->publishTransforms(positions, ros::Time::now());
-
-        // Process a round of subscription messages
-        ros::spinOnce();
-
-        // This will adjust as needed per iteration
-        loop_rate.sleep();
-    }
-
-
-    return 0;
-}

joint_state_publisher/joint_state_publisher

@@ -10,13 +10,22 @@ from threading import Thread
 
 RANGE = 10000
 
+def get_param(name, value=None):
+    private = "~%s" % name
+    if rospy.has_param(private):
+        return rospy.get_param(private)
+    elif rospy.has_param(name):
+        return rospy.get_param(name)
+    else:
+        return value
+
 class JointStatePublisher():
     def __init__(self):
-        description = rospy.get_param("robot_description")
+        description = get_param('robot_description')
         robot = xml.dom.minidom.parseString(description).getElementsByTagName('robot')[0]
         self.free_joints = {}
         self.joint_list = [] # for maintaining the original order of the joints
-        self.dependent_joints = rospy.get_param("dependent_joints", {})
+        self.dependent_joints = get_param("dependent_joints", {})
         
         # Find all non-fixed joints
         for child in robot.childNodes:
@@ -46,10 +55,36 @@ class JointStatePublisher():
                 self.free_joints[name] = joint
                 self.joint_list.append(name)
 
+        use_gui = get_param("use_gui", False)
+
+        if use_gui:
+            app = wx.App()
+            self.gui = JointStatePublisherGui("Joint State Publisher", self)
+            self.gui.Show()
+            Thread(target=app.MainLoop).start()
+        else:
+            self.gui = None
+
+        source_list = get_param("source_list", [])
+        self.sources = []
+        for source in source_list:
+            self.sources.append(rospy.Subscriber(source, JointState, self.source_cb))
+
         self.pub = rospy.Publisher('joint_states', JointState)
 
+    def source_cb(self, msg):
+        for i in range(len(msg.name)):
+            name = msg.name[i]
+            position = msg.position[i]
+            if name in self.free_joints:
+                joint = self.free_joints[name]
+                joint['value'] = position
+        if self.gui is not None:
+            self.gui.update_sliders()
+        
+
     def loop(self):
-        hz = rospy.get_param("rate", 10) # 10hz
+        hz = get_param("rate", 10) # 10hz
         r = rospy.Rate(hz) 
 
         # Publish Joint States
@@ -129,8 +164,14 @@ class JointStatePublisherGui(wx.Frame):
             joint = joint_info['joint']
             value = self.sliderToValue(purevalue, joint)
             joint['value'] = value
-            joint_info['slider'].SetValue(purevalue)
+        self.update_sliders()
-            joint_info['display'].SetValue("%.2f"%value)
+
+    def update_sliders(self):
+        for (name,joint_info) in self.joint_map.items():
+            joint = joint_info['joint']
+            joint_info['slidervalue'] = self.valueToSlider(joint['value'], joint)
+            joint_info['slider'].SetValue(joint_info['slidervalue'])
+            joint_info['display'].SetValue("%.2f"%joint['value'])
 
     def center_event(self, event):
         self.center()
@@ -159,14 +200,6 @@ if __name__ == '__main__':
     try:
         rospy.init_node('joint_state_publisher')
         jsp = JointStatePublisher()
-
-        use_gui = rospy.get_param("use_gui", False)
-        if use_gui:
-            app = wx.App()
-            gui = JointStatePublisherGui("Joint State Publisher", jsp)
-            gui.Show()
-            Thread(target=app.MainLoop).start()
-        
         jsp.loop()
         
     except rospy.ROSInterruptException: pass

stack.xml

@@ -4,6 +4,10 @@
   <license>BSD</license>  
   <review status="unreviewed" notes=""/>
   <url>http://ros.org/wiki/urdf_tools</url>
-  <depend stack="ros" />
+  <depend stack="common_msgs" /> <!-- sensor_msgs -->
+  <depend stack="geometry" /> <!-- tf, tf_conversions -->
+  <depend stack="kinematics" /> <!-- kinematics_msgs -->
+  <depend stack="robot_model" /> <!-- robot_state_publisher, urdf, kdl_parser -->
+  <depend stack="ros" /> <!-- rospy, roscpp -->
 
 </stack>