Gavin,<div><br></div><div>There are a couple problems I see after a brief look:</div><div><br></div><div>1) You seem to be publishing the same TF link (odom->world) from both publishers. This may not be what's causing your problem, but it's also not good.</div>
<div>2) In your second program, you're creating the Publisher object inside your subscription callback. You need to be doing this when you initially setup the node and pass it in -- otherwise the message is very unlikely to make it out to any subscribers. My guess is that this is the root of your problem.</div>
<div><br></div><div>Josh<br><br><div class="gmail_quote">On Thu, Sep 23, 2010 at 12:08 PM, Gavin McCarter <span dir="ltr"><<a href="mailto:gmccarter88@gmail.com">gmccarter88@gmail.com</a>></span> wrote:<br><blockquote class="gmail_quote" style="margin:0 0 0 .8ex;border-left:1px #ccc solid;padding-left:1ex;">
Hey Everyone,<br>I'm working with models in Rviz and since I've upgraded from boxturtle to cturtle, I've been having trouble getting them to appear in rviz. I think I've isolated the issue: I'm using a state-publisher to send joint states to a another state publisher that is subscribed to the original state-publisher and assigns the joint states defined in the state publisher to joints that do not appear in the state publisher. From there they are supposedly sent off to the robot_state_publisher. My issue is that there appears to be disconnect between either state publisher #1 and #2 or the #2 state publisher and the robot_state_publisher. I've been able to make objects appear in Rviz when I only use the original state publisher but not when I add in the #2 state publisher. I'm not sure what's causing this but I would appriciate some help. I've attached the two files below. I'm running Ubuntu 10.4<br>
<br>Thanks in advance,<br>-Gavin<br><br>Here's the urdf I'm using too:<br><robot name="sparky"><br><br><resource type="stl_meshes" location="/home/gavin/usc-cturtle/stacks/interaction_ros_pkg/sparky/sparky_model/meshes<br>
"/><br><br><!---------------------Sparky Body ------------------------------><br><br><link name="world" /><br><br><br><link name="centerplate"><br><br><inertial><br> <mass value="5"/><br>
<inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br>
<material name="grey" ><br> <color rgba=".2 .2 .2 .2" /><br> </material><br> <geometry><br> <sphere radius="5"/><br> </geometry><br>
</visual><br></link><br><br><br><link name="stand"><br><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 -1.57" /><br> <material name="grey" ><br> <color rgba=".2 .2 .2 .2" /><br>
</material><br> <geometry><br><sphere radius="5"/><br> </geometry><br> </visual><br> <br></link> <br><br> <br><link name="hips"><br><br><inertial><br>
<mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br> </inertial><br>
<visual><br> <origin xyz="0 0 0" rpy="0 1.57 1.57" /><br> <material name="grey" ><br> <color rgba=".2 .2 .2 .2" /><br> </material> <br>
<geometry><br> <sphere radius="5"/><br> </geometry><br> </visual><br></link><br><br><joint name="worldjoint" type="fixed"><br> <origin xyz="0 0 0" rpy="0 0 0"/><br>
<parent link="world"/><br> <child link="stand"/><br></joint><br><br><joint name="hipjoint" type="fixed"><br> <origin xyz="-.1 .7 -.5" rpy="0 0 0"/><br>
<parent link="stand"/><br> <child link="hips"/><br></joint><br><br><br><br><br><!--<br>-----------------------------Sparky Lower Spine ------------------------------<br>--><br>
<br><link name="LS1"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> <color rgba=".5 .5 .5 .5" /><br>
</material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br></link><br><br><br><br><link name="LS2"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br></link><br><br><link name="LS3"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br>
<cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br></link><br><br><link name="LS4"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br></link><br><br><link name="LS5"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br>
<cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br></link><br><br><br><link name="LS6"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br></link><br><br><link name="LS7"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br>
<cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br> <br></link><br><br><link name="LS8"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br> <br></link><br><br><link name="LS9"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> <color rgba=".5 .5 .5 .5" /><br>
</material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br></link><br><br><br><br><link name="LS10"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br></link><br><br><link name="LS11"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br>
<cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br></link><br><br><link name="LS12"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br></link><br><br><link name="LS13"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br>
<cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br></link><br><br><link name="LS14"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br></link><br><br><link name="LS15"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br>
<cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br> <br></link><br><br><link name="LS16"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br> <br></link><br><link name="LS17"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> <color rgba=".5 .5 .5 .5" /><br>
</material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br></link><br><br><br><br><link name="LS18"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br></link><br><br><link name="LS19"><br><inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br>
<origin/><br> </inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br>
<cylinder radius=".25" length=".098" /><br> </geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br>
<geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br> </geometry><br> </collision><br></link><br><br><link name="LS20"><br>
<inertial><br> <mass value="5"/><br> <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0" izz="100" /><br> <origin/><br>
</inertial><br> <visual><br> <origin xyz="0 0 0" rpy="0 0 0" /><br> <material name="silver" ><br> </material><br> <geometry><br> <cylinder radius=".25" length=".098" /><br>
</geometry><br> </visual><br> <collision><br> <origin xyz="0 0 0" rpy="0 0 0 " /><br> <geometry name="collision_geom"><br> <box size="0.01 0.01 0.01" /><br>
</geometry><br> </collision><br></link><br><br><br><br><joint name="LSJ1" type="fixed"><br> <parent link="hips" /><br> <child link="LS1" /><br>
<origin xyz="0 0 1.3" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ2" type="fixed"><br> <parent link="LS1" /><br>
<child link="LS2" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ3" type="fixed"><br>
<parent link="LS2" /><br> <child link="LS3" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br>
<br><joint name="LSJ4" type="fixed"><br> <parent link="LS3" /><br> <child link="LS4" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br>
<axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ5" type="fixed"><br> <parent link="LS4" /><br> <child link="LS5" /><br>
<origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ6" type="fixed"><br> <parent link="LS5" /><br>
<child link="LS6" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ7" type="fixed"><br>
<parent link="LS6" /><br> <child link="LS7" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br>
<br><joint name="LSJ8" type="fixed"><br> <parent link="LS7" /><br> <child link="LS8" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br>
<axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ9" type="fixed"><br> <parent link="LS8" /><br> <child link="LS9" /><br>
<origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ10" type="fixed"><br> <parent link="LS9" /><br>
<child link="LS10" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ11" type="fixed"><br>
<parent link="LS10" /><br> <child link="LS11" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br>
<br><joint name="LSJ12" type="fixed"><br> <parent link="LS11" /><br> <child link="LS12" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br>
<axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ13" type="fixed"><br> <parent link="LS12" /><br> <child link="LS13" /><br>
<origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ14" type="fixed"><br> <parent link="LS13" /><br>
<child link="LS14" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ15" type="fixed"><br>
<parent link="LS14" /><br> <child link="LS15" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br>
<br><joint name="LSJ16" type="fixed"><br> <parent link="LS15" /><br> <child link="LS16" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br>
<axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ17" type="fixed"><br> <parent link="LS16" /><br> <child link="LS17" /><br>
<origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ18" type="fixed"><br> <parent link="LS17" /><br>
<child link="LS18" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br><joint name="LSJ19" type="fixed"><br>
<parent link="LS18" /><br> <child link="LS19" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br>
<br><joint name="TorsoBend" type="continuous"><br> <parent link="LS19" /><br> <child link="LS20" /><br> <origin xyz="0 0 .049" rpy="0 0 0" /><br>
<axis xyz="1 0 0" /> <br></joint><br><br><br><joint name="LSJ21" type="fixed"><br> <parent link="LS20" /><br> <child link="centerplate" /><br>
<origin xyz="0 0 .42" rpy="0 0 0" /><br> <axis xyz="1 0 0" /> <br></joint><br><br></robot><br><br><br>------Original State Publisher----------------<br><br>
#include <string><br>#include <ros/ros.h><br>#include <sensor_msgs/JointState.h><br>#include <tf/transform_broadcaster.h><br>#include <sstream><br>#include <std_msgs/String.h><br><br><br>
int main(int argc, char** argv) {<br> ros::init(argc, argv, "intial_state_publisher");<br> ros::NodeHandle n;<br> ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("hi", 1);<br>
tf::TransformBroadcaster broadcaster;<br> ros::Rate loop_rate(30);<br><br><br> // robot state<br> double HeadNod=0, HeadTurn =0, Mouth=0, EyesLtRt=0, <br> EyeBlink=0, LtArmOut=0, LtArmForward=0, RtArmForward=0,<br>
RtArmOut=0, LtElbow=0, RtElbow=0, LtWrist=0, RtWrist=0,<br> LtFootForward=0, RtFootForward=0, LtFootUp=0, RtFootUp=0,<br> TorsoBend=0, angle=0;<br> <br> // message declarations<br> geometry_msgs::TransformStamped odom_trans;<br>
sensor_msgs::JointState joint_state;<br> odom_trans.header.frame_id = "odom";<br> odom_trans.child_frame_id = "world";<br><br><br> while (ros::ok()) {<br> joint_state.header.stamp = ros::Time::now();<br>
joint_state.name.resize(18);<br> joint_state.position.resize(18);<br> <br> //head<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[0] ="HeadNod";<br>
joint_state.position[0] = HeadNod;<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[1] ="HeadTurn";<br> joint_state.position [1] =HeadTurn; <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[2] ="Mouth";<br>
joint_state.position[2] =Mouth;<br> <br> //eyes<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[3] ="EyesLtRt";<br> joint_state.position[3] =EyesLtRt;<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[4] ="EyeBlink";<br>
joint_state.position [4] =EyeBlink;<br> <br> //left arm <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[5] ="LtArmOut";<br> joint_state.position[5] =LtArmOut;<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a> [6]="LtArmForward";<br> joint_state.position[6]=LtArmForward; <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[7] ="LtElbow";<br>
joint_state.position [7] =LtElbow; <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[8] ="LtWrist";<br> joint_state.position [8] =LtWrist;<br> <br> //right arm<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[9] ="RtArmOut";<br> joint_state.position[9] =RtArmOut; <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[10] ="RtArmForward";<br>
joint_state.position[10] =RtArmForward;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[11] ="RtElbow";<br> joint_state.position [11] =RtElbow;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[12] ="RtWrist";<br>
joint_state.position [12] =RtWrist;<br> <br> //left leg<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[13] ="LtFootForward";<br> joint_state.position [13] =LtFootForward;<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[14] ="LtFootUp";<br> joint_state.position [14] =LtFootUp; <br> <br> //right leg<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[15] ="RtFootForward";<br>
joint_state.position [15] =RtFootForward;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[16] ="RtFootUp";<br> joint_state.position [16] =RtFootUp;<br> <br>
//spine<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[17] ="TorsoBend";<br> joint_state.position [17] =TorsoBend;<br><br><br><br><br><br><br> // update transform<br> <br> <br>
//send the joint state and transform<br>
// update transform<br> // (moving in a circle with radius=2)<br> odom_trans.header.stamp = ros::Time::now();<br> odom_trans.transform.translation.x = cos(angle)*2;<br> odom_trans.transform.translation.y = sin(angle)*2;<br>
odom_trans.transform.translation.z = .7;<br> odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);<br><br> //send the joint state and transform<br><br> joint_pub.publish(joint_state); <br>
broadcaster.sendTransform(odom_trans);<br> ros::spinOnce();<br><br><br> // This will adjust as needed per iteration<br> loop_rate.sleep();<br> }<br><br><br> return 0;<br>}<br><br>----------Second State Publisher--------------------<br>
<br>#include <string><br>#include <ros/ros.h><br>#include <sensor_msgs/JointState.h><br>#include <tf/transform_broadcaster.h><br><br><br>using namespace std;<br>using namespace ros;<br><br>void jointCallback(const sensor_msgs::JointStateConstPtr& state)<br>
{<br> <br> <br> ros::NodeHandle n;<br> ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);<br><br> tf::TransformBroadcaster broadcaster;<br> ros::Rate loop_rate(30);<br>
<br> geometry_msgs::TransformStamped odom_trans;<br> sensor_msgs::JointState joint_state;<br> odom_trans.header.frame_id = "odom";<br> odom_trans.child_frame_id = "world";<br><br><br><br>
double HeadNod, Mouth, EyesLtRt, EyeBlink, LtArmOut, <br> LtArmForward, RtArmForward, RtArmOut, LtElbow, RtElbow, LtWrist,<br> RtWrist, LtFootForward, RtFootForward, LtFootUp, RtFootUp, <br> TorsoBend, HeadTurn, angle=0;<br>
<br> float radcorrect=3.14159f/180.0f;<br><br> joint_state.header.stamp = ros::Time::now();<br> joint_state.name.resize(64);<br> joint_state.position.resize(64);<br> <br> <br> ROS_INFO("running ok");<br>
<br> //head<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[0] ="HeadNod";<br> HeadNod = radcorrect*state->position[0];<br> joint_state.position[0] = HeadNod;<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a> [1] ="HeadTurn";<br> HeadTurn= radcorrect*state->position[1];<br> joint_state.position [1] =HeadTurn;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[2] ="Mouth";<br>
Mouth = radcorrect*state->position[2];<br> joint_state.position[2] = Mouth; <br> <br> //eyes<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[3] ="EyesLtRt";<br>
EyesLtRt = radcorrect*state->position[3];<br> joint_state.position[3] =EyesLtRt;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a> [4] ="LeyeJoint";<br> joint_state.position [4] =EyesLtRt; <br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[5] ="EyeBlink";<br> EyeBlink = radcorrect*(90-state->position[4]);<br> joint_state.position [5] =-EyeBlink;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[6] ="RLidJoint";<br>
joint_state.position [6] =-EyeBlink; <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[7] ="LPinJoint";<br> joint_state.position[7] = -1*(EyesLtRt);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[8] ="EyeBeamJoint";<br>
joint_state.position[8]=.5113*(EyesLtRt);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[9] ="EyeBallPinJoint";<br> joint_state.position[9]=(EyesLtRt);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[10] ="EyeBallPinJoint2";<br>
joint_state.position[10]=(EyesLtRt);<br> <br> //left arm<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[11] ="LtArmOut";<br> LtArmOut = radcorrect*state->position[5];<br>
joint_state.position[11] =LtArmOut;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a> [12]="LtArmForward";<br> LtArmForward = radcorrect*state->position[6];<br>
joint_state.position[12]=LtArmForward;<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[13] ="LtElbow";<br> LtElbow = radcorrect*state->position[7];<br> joint_state.position [13] =-LtElbow; <br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[14] ="LtWrist";<br>
LtWrist = radcorrect*state->position[8];<br> joint_state.position [14] =LtWrist;<br> <br> //right arm <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[15] ="RtArmOut";<br>
RtArmOut = radcorrect*state->position[9];<br> joint_state.position[15] =-RtArmOut; <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[16] ="RtArmForward";<br>
RtArmForward = radcorrect*state->position[10];<br>
joint_state.position[16] =RtArmForward;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[17] ="RtElbow";<br> RtElbow = radcorrect*state->position[11];<br> joint_state.position [17] =RtElbow;<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[18] ="RtWrist";<br> RtWrist = radcorrect*state->position[12];<br> joint_state.position [18] =-RtWrist;<br><br> //left leg<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[19] ="LtFootForward";<br> LtFootForward = radcorrect*state->position[13];<br> joint_state.position [19] =LtFootForward;<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[20] ="LtFootUp";<br>
LtFootUp = radcorrect*state->position[14];<br> joint_state.position [20] =-LtFootUp;<br> <br> //right leg<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[21] ="RtFootForward";<br>
RtFootForward = radcorrect*state->position[15];<br> joint_state.position [21] =RtFootForward;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[22] ="RtFootUp";<br>
RtFootUp = radcorrect*state->position[16];<br>
joint_state.position [22] =-RtFootUp;<br> <br> //upper spine<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a> [23] ="TorsoBend";<br> TorsoBend= radcorrect*state->position[17];<br>
joint_state.position [23] =TorsoBend/41;<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[24] ="TSJ2";<br> joint_state.position [24] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[25] ="TSJ3";<br>
joint_state.position [25] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[26] ="TSJ4";<br> joint_state.position [26] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[27] ="TSJ5";<br>
joint_state.position [27] =-(TorsoBend/41); <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[28] ="TSJ6";<br> joint_state.position [28] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[29] ="TSJ7";<br>
joint_state.position [29] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[30] ="TSJ8";<br> joint_state.position [30] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[31] ="TSJ9";<br>
joint_state.position [31] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[32] ="TSJ10";<br> joint_state.position [32] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[33] ="TSJ11";<br>
joint_state.position [33] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[34] ="TSJ12";<br> joint_state.position [34] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[35] ="TSJ13";<br>
joint_state.position [35] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[36] ="TSJ14";<br> joint_state.position [36] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[37] ="TSJ15";<br>
joint_state.position [37] =-(TorsoBend/41); <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[38] ="TSJ16";<br> joint_state.position [38] =-(TorsoBend/41);<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[39] ="TSJ17";<br>
joint_state.position [39] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[40] ="TSJ18";<br> joint_state.position [40] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[41] ="TSJ19";<br>
joint_state.position [41] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[42] ="TSJ20";<br> joint_state.position [42] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[43] ="TSJ21";<br>
joint_state.position [43] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[44] ="TSJ22";<br> joint_state.position [44] =-(TorsoBend/41); <br>
<br> //lower spine<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[45] ="LSJ2";<br> joint_state.position [45] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[46] ="LSJ3";<br>
joint_state.position [46] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[47] ="LSJ4";<br> joint_state.position [47] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[48] ="LSJ5";<br>
joint_state.position [48] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[49] ="LSJ6";<br> joint_state.position [49] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[50] ="LSJ7";<br>
joint_state.position [50] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[51] ="LSJ8";<br> joint_state.position [51] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[52] ="LSJ9";<br>
joint_state.position [52] =-(TorsoBend/41); <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[53] ="LSJ10";<br> joint_state.position [53] =-(TorsoBend/41);<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[54] ="LSJ11";<br>
joint_state.position [54] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[55] ="LSJ12";<br> joint_state.position [55] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[56] ="LSJ13";<br>
joint_state.position [56] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[57] ="LSJ14";<br> joint_state.position [57] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[58] ="LSJ15";<br>
joint_state.position [58] =-(TorsoBend/41); <br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[59] ="LSJ16";<br> joint_state.position [59] =-(TorsoBend/41);<br>
<a href="http://joint_state.name" target="_blank">joint_state.name</a>[60] ="LSJ17";<br>
joint_state.position [60] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[61] ="LSJ18";<br> joint_state.position [61] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[62] ="LSJ19";<br>
joint_state.position [62] =-(TorsoBend/41);<br> <a href="http://joint_state.name" target="_blank">joint_state.name</a>[63] ="LSJ20";<br> joint_state.position [63] =-(TorsoBend/41);<br> <br>
<br><br>map<string, double> joint_positions;<br>
for (unsigned int i=0; i<state->name.size(); i++) <br> joint_positions.insert(make_pair(state->name[i], state->position[i]));<br><br><br><br><br>odom_trans.header.stamp = ros::Time::now();<br> odom_trans.transform.translation.x = cos(angle)*2;<br>
odom_trans.transform.translation.y = sin(angle)*2;<br> odom_trans.transform.translation.z = .7;<br> odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);<br><br><br> joint_pub.publish(joint_state);<br>
broadcaster.sendTransform(odom_trans);<br><br><br>}<br><br><br>int main(int argc, char** argv)<br>{<br> ros::init(argc, argv, "state_publisher");<br> ros::NodeHandle n;<br> ros::Subscriber joint_sub = n.subscribe("hi", 10, jointCallback);<br>
tf::TransformBroadcaster broadcaster;<br>ros::spin();<br>}<br><br><br> <br><br>
<br>_______________________________________________<br>
ros-users mailing list<br>
<a href="mailto:ros-users@code.ros.org">ros-users@code.ros.org</a><br>
<a href="https://code.ros.org/mailman/listinfo/ros-users" target="_blank">https://code.ros.org/mailman/listinfo/ros-users</a><br>
<br></blockquote></div><br></div>