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