diff --git a/easy_state_publisher/CMakeLists.txt b/easy_state_publisher/CMakeLists.txt deleted file mode 100644 index a351fcd..0000000 --- a/easy_state_publisher/CMakeLists.txt +++ /dev/null @@ -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) - diff --git a/easy_state_publisher/Makefile b/easy_state_publisher/Makefile deleted file mode 100644 index b75b928..0000000 --- a/easy_state_publisher/Makefile +++ /dev/null @@ -1 +0,0 @@ -include $(shell rospack find mk)/cmake.mk \ No newline at end of file diff --git a/easy_state_publisher/mainpage.dox b/easy_state_publisher/mainpage.dox deleted file mode 100644 index 48fd4ab..0000000 --- a/easy_state_publisher/mainpage.dox +++ /dev/null @@ -1,26 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b easy_state_publisher is ... - - - - -\section codeapi Code API - - - - -*/ diff --git a/easy_state_publisher/manifest.xml b/easy_state_publisher/manifest.xml deleted file mode 100644 index 3083440..0000000 --- a/easy_state_publisher/manifest.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - Easy State Publisher publishes JointState and/or TF messages for a given robot model - - David Lu!! - BSD - - http://ros.org/wiki/easy_state_publisher - - - - - - - - diff --git a/easy_state_publisher/src/easy_state_publisher.cpp b/easy_state_publisher/src/easy_state_publisher.cpp deleted file mode 100644 index 784ad52..0000000 --- a/easy_state_publisher/src/easy_state_publisher.cpp +++ /dev/null @@ -1,58 +0,0 @@ -#include -#include -#include -#include -#include -#include - -int main(int argc, char** argv) { - ros::init(argc, argv, "easy_state_publisher"); - ros::NodeHandle n; - ros::Publisher m_joint_pub = n.advertise - ("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 > joint_map; - typedef std::map 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; -} diff --git a/easy_state_publisher/src/easy_tf_publisher.cpp b/easy_state_publisher/src/easy_tf_publisher.cpp deleted file mode 100644 index 72e4a6b..0000000 --- a/easy_state_publisher/src/easy_tf_publisher.cpp +++ /dev/null @@ -1,50 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -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 > joint_map; - - std::map 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; -} diff --git a/joint_state_publisher/joint_state_publisher b/joint_state_publisher/joint_state_publisher index 4ffabba..ae5aeb2 100755 --- a/joint_state_publisher/joint_state_publisher +++ b/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) - joint_info['display'].SetValue("%.2f"%value) + self.update_sliders() + + 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 diff --git a/stack.xml b/stack.xml index 5082fd8..7d7b363 100644 --- a/stack.xml +++ b/stack.xml @@ -4,6 +4,10 @@ BSD http://ros.org/wiki/urdf_tools - + + + + +