bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updated meta-data, JointStatePublisher with merging ability

This commit is contained in:
probablydavid 2011-02-21 23:26:39 +00:00
parent d66f558a6e
commit a3c35dc515
8 changed files with 51 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
easy_state_publisher/CMakeLists.txt
View File

@ -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)

1
easy_state_publisher/Makefile
View File

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

26
easy_state_publisher/mainpage.dox
View File

@ -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.
-->
*/

16
easy_state_publisher/manifest.xml
View File

@ -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>

58
easy_state_publisher/src/easy_state_publisher.cpp
View File

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

50
easy_state_publisher/src/easy_tf_publisher.cpp
View File

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

59
joint_state_publisher/joint_state_publisher
View File

@ -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

6
stack.xml
View File

@ -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>