> Hey Everyone,
> 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
>
> Thanks in advance,
> -Gavin
>
> Here's the urdf I'm using too:
> <robot name="sparky">
>
> <resource type="stl_meshes"
> location="/home/gavin/usc-cturtle/stacks/interaction_ros_pkg/sparky/sparky_model/meshes
> "/>
>
> <!---------------------Sparky Body ------------------------------>
>
> <link name="world" />
>
>
> <link name="centerplate">
>
> <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>
>
>
> <link name="stand">
>
> <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>
>
> </link>
>
>
> <link name="hips">
>
> <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>
>
> <joint name="worldjoint" type="fixed">
> <origin xyz="0 0 0" rpy="0 0 0"/>
> <parent link="world"/>
> <child link="stand"/>
> </joint>
>
> <joint name="hipjoint" type="fixed">
> <origin xyz="-.1 .7 -.5" rpy="0 0 0"/>
> <parent link="stand"/>
> <child link="hips"/>
> </joint>
>
>
>
>
> <!--
> -----------------------------Sparky Lower Spine
> ------------------------------
> -->
>
> <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>
>
>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
>
> <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>
>
> <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>
>
> </link>
>
> <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>
>
> </link>
>
> <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>
>
>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> </link>
>
> <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>
>
> </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>
>
>
>
> <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>
>
> <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>
>
> <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>
>
>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
> <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>
>
>
> <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>
>
> </robot>
>
>
> ------Original State Publisher----------------
>
> #include <string>
> #include <ros/ros.h>
> #include <sensor_msgs/JointState.h>
> #include <tf/transform_broadcaster.h>
> #include <sstream>
> #include <std_msgs/String.h>
>
>
> 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);
>
>
> // 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;
>
> // message declarations
> geometry_msgs::TransformStamped odom_trans;
> sensor_msgs::JointState joint_state;
> odom_trans.header.frame_id = "odom";
> odom_trans.child_frame_id = "world";
>
>
> while (ros::ok()) {
> joint_state.header.stamp = ros::Time::now();
> joint_state.name.resize(18);
> joint_state.position.resize(18);
>
> //head
> joint_state.name[0] ="HeadNod";
> joint_state.position[0] = HeadNod;
> joint_state.name[1] ="HeadTurn";
> joint_state.position [1] =HeadTurn;
> joint_state.name[2] ="Mouth";
> joint_state.position[2] =Mouth;
>
> //eyes
> joint_state.name[3] ="EyesLtRt";
> joint_state.position[3] =EyesLtRt;
> joint_state.name[4] ="EyeBlink";
> joint_state.position [4] =EyeBlink;
>
> //left arm
> joint_state.name[5] ="LtArmOut";
> joint_state.position[5] =LtArmOut;
> joint_state.name [6]="LtArmForward";
> joint_state.position[6]=LtArmForward;
> joint_state.name[7] ="LtElbow";
> joint_state.position [7] =LtElbow;
> joint_state.name[8] ="LtWrist";
> joint_state.position [8] =LtWrist;
>
> //right arm
> joint_state.name[9] ="RtArmOut";
> joint_state.position[9] =RtArmOut;
> joint_state.name[10] ="RtArmForward";
> joint_state.position[10] =RtArmForward;
> joint_state.name[11] ="RtElbow";
> joint_state.position [11] =RtElbow;
> joint_state.name[12] ="RtWrist";
> joint_state.position [12] =RtWrist;
>
> //left leg
> joint_state.name[13] ="LtFootForward";
> joint_state.position [13] =LtFootForward;
> joint_state.name[14] ="LtFootUp";
> joint_state.position [14] =LtFootUp;
>
> //right leg
> joint_state.name[15] ="RtFootForward";
> joint_state.position [15] =RtFootForward;
> joint_state.name[16] ="RtFootUp";
> joint_state.position [16] =RtFootUp;
>
> //spine
> joint_state.name[17] ="TorsoBend";
> joint_state.position [17] =TorsoBend;
>
>
>
>
>
>
> // update transform
>
>
> //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);
>
> //send the joint state and transform
>
> joint_pub.publish(joint_state);
> broadcaster.sendTransform(odom_trans);
> ros::spinOnce();
>
>
> // This will adjust as needed per iteration
> loop_rate.sleep();
> }
>
>
> return 0;
> }
>
> ----------Second State Publisher--------------------
>
> #include <string>
> #include <ros/ros.h>
> #include <sensor_msgs/JointState.h>
> #include <tf/transform_broadcaster.h>
>
>
> using namespace std;
> using namespace ros;
>
> void jointCallback(const sensor_msgs::JointStateConstPtr& state)
> {
>
>
> ros::NodeHandle n;
> ros::Publisher joint_pub =
> n.advertise<sensor_msgs::JointState>("joint_states", 1);
>
> tf::TransformBroadcaster broadcaster;
> ros::Rate loop_rate(30);
>
> geometry_msgs::TransformStamped odom_trans;
> sensor_msgs::JointState joint_state;
> odom_trans.header.frame_id = "odom";
> odom_trans.child_frame_id = "world";
>
>
>
> double HeadNod, Mouth, EyesLtRt, EyeBlink, LtArmOut,
> LtArmForward, RtArmForward, RtArmOut, LtElbow, RtElbow, LtWrist,
> RtWrist, LtFootForward, RtFootForward, LtFootUp, RtFootUp,
> TorsoBend, HeadTurn, angle=0;
>
> float radcorrect=3.14159f/180.0f;
>
> joint_state.header.stamp = ros::Time::now();
> joint_state.name.resize(64);
> joint_state.position.resize(64);
>
>
> ROS_INFO("running ok");
>
> //head
> joint_state.name[0] ="HeadNod";
> HeadNod = radcorrect*state->position[0];
> joint_state.position[0] = HeadNod;
> joint_state.name [1] ="HeadTurn";
> HeadTurn= radcorrect*state->position[1];
> joint_state.position [1] =HeadTurn;
> joint_state.name[2] ="Mouth";
> Mouth = radcorrect*state->position[2];
> joint_state.position[2] = Mouth;
>
> //eyes
> joint_state.name[3] ="EyesLtRt";
> EyesLtRt = radcorrect*state->position[3];
> joint_state.position[3] =EyesLtRt;
> joint_state.name [4] ="LeyeJoint";
> joint_state.position [4] =EyesLtRt;
> joint_state.name[5] ="EyeBlink";
> EyeBlink = radcorrect*(90-state->position[4]);
> joint_state.position [5] =-EyeBlink;
> joint_state.name[6] ="RLidJoint";
> joint_state.position [6] =-EyeBlink;
> joint_state.name[7] ="LPinJoint";
> joint_state.position[7] = -1*(EyesLtRt);
> joint_state.name[8] ="EyeBeamJoint";
> joint_state.position[8]=.5113*(EyesLtRt);
> joint_state.name[9] ="EyeBallPinJoint";
> joint_state.position[9]=(EyesLtRt);
> joint_state.name[10] ="EyeBallPinJoint2";
> joint_state.position[10]=(EyesLtRt);
>
> //left arm
> joint_state.name[11] ="LtArmOut";
> LtArmOut = radcorrect*state->position[5];
> joint_state.position[11] =LtArmOut;
> joint_state.name [12]="LtArmForward";
> LtArmForward = radcorrect*state->position[6];
> joint_state.position[12]=LtArmForward;
> joint_state.name[13] ="LtElbow";
> LtElbow = radcorrect*state->position[7];
> joint_state.position [13] =-LtElbow;
> joint_state.name[14] ="LtWrist";
> LtWrist = radcorrect*state->position[8];
> joint_state.position [14] =LtWrist;
>
> //right arm
> joint_state.name[15] ="RtArmOut";
> RtArmOut = radcorrect*state->position[9];
> joint_state.position[15] =-RtArmOut;
> joint_state.name[16] ="RtArmForward";
> RtArmForward = radcorrect*state->position[10];
> joint_state.position[16] =RtArmForward;
> joint_state.name[17] ="RtElbow";
> RtElbow = radcorrect*state->position[11];
> joint_state.position [17] =RtElbow;
> joint_state.name[18] ="RtWrist";
> RtWrist = radcorrect*state->position[12];
> joint_state.position [18] =-RtWrist;
>
> //left leg
> joint_state.name[19] ="LtFootForward";
> LtFootForward = radcorrect*state->position[13];
> joint_state.position [19] =LtFootForward;
> joint_state.name[20] ="LtFootUp";
> LtFootUp = radcorrect*state->position[14];
> joint_state.position [20] =-LtFootUp;
>
> //right leg
> joint_state.name[21] ="RtFootForward";
> RtFootForward = radcorrect*state->position[15];
> joint_state.position [21] =RtFootForward;
> joint_state.name[22] ="RtFootUp";
> RtFootUp = radcorrect*state->position[16];
> joint_state.position [22] =-RtFootUp;
>
> //upper spine
> joint_state.name [23] ="TorsoBend";
> TorsoBend= radcorrect*state->position[17];
> joint_state.position [23] =TorsoBend/41;
> joint_state.name[24] ="TSJ2";
> joint_state.position [24] =-(TorsoBend/41);
> joint_state.name[25] ="TSJ3";
> joint_state.position [25] =-(TorsoBend/41);
> joint_state.name[26] ="TSJ4";
> joint_state.position [26] =-(TorsoBend/41);
> joint_state.name[27] ="TSJ5";
> joint_state.position [27] =-(TorsoBend/41);
> joint_state.name[28] ="TSJ6";
> joint_state.position [28] =-(TorsoBend/41);
> joint_state.name[29] ="TSJ7";
> joint_state.position [29] =-(TorsoBend/41);
> joint_state.name[30] ="TSJ8";
> joint_state.position [30] =-(TorsoBend/41);
> joint_state.name[31] ="TSJ9";
> joint_state.position [31] =-(TorsoBend/41);
> joint_state.name[32] ="TSJ10";
> joint_state.position [32] =-(TorsoBend/41);
> joint_state.name[33] ="TSJ11";
> joint_state.position [33] =-(TorsoBend/41);
> joint_state.name[34] ="TSJ12";
> joint_state.position [34] =-(TorsoBend/41);
> joint_state.name[35] ="TSJ13";
> joint_state.position [35] =-(TorsoBend/41);
> joint_state.name[36] ="TSJ14";
> joint_state.position [36] =-(TorsoBend/41);
> joint_state.name[37] ="TSJ15";
> joint_state.position [37] =-(TorsoBend/41);
> joint_state.name[38] ="TSJ16";
> joint_state.position [38] =-(TorsoBend/41);
> joint_state.name[39] ="TSJ17";
> joint_state.position [39] =-(TorsoBend/41);
> joint_state.name[40] ="TSJ18";
> joint_state.position [40] =-(TorsoBend/41);
> joint_state.name[41] ="TSJ19";
> joint_state.position [41] =-(TorsoBend/41);
> joint_state.name[42] ="TSJ20";
> joint_state.position [42] =-(TorsoBend/41);
> joint_state.name[43] ="TSJ21";
> joint_state.position [43] =-(TorsoBend/41);
> joint_state.name[44] ="TSJ22";
> joint_state.position [44] =-(TorsoBend/41);
>
> //lower spine
> joint_state.name[45] ="LSJ2";
> joint_state.position [45] =-(TorsoBend/41);
> joint_state.name[46] ="LSJ3";
> joint_state.position [46] =-(TorsoBend/41);
> joint_state.name[47] ="LSJ4";
> joint_state.position [47] =-(TorsoBend/41);
> joint_state.name[48] ="LSJ5";
> joint_state.position [48] =-(TorsoBend/41);
> joint_state.name[49] ="LSJ6";
> joint_state.position [49] =-(TorsoBend/41);
> joint_state.name[50] ="LSJ7";
> joint_state.position [50] =-(TorsoBend/41);
> joint_state.name[51] ="LSJ8";
> joint_state.position [51] =-(TorsoBend/41);
> joint_state.name[52] ="LSJ9";
> joint_state.position [52] =-(TorsoBend/41);
> joint_state.name[53] ="LSJ10";
> joint_state.position [53] =-(TorsoBend/41);
> joint_state.name[54] ="LSJ11";
> joint_state.position [54] =-(TorsoBend/41);
> joint_state.name[55] ="LSJ12";
> joint_state.position [55] =-(TorsoBend/41);
> joint_state.name[56] ="LSJ13";
> joint_state.position [56] =-(TorsoBend/41);
> joint_state.name[57] ="LSJ14";
> joint_state.position [57] =-(TorsoBend/41);
> joint_state.name[58] ="LSJ15";
> joint_state.position [58] =-(TorsoBend/41);
> joint_state.name[59] ="LSJ16";
> joint_state.position [59] =-(TorsoBend/41);
> joint_state.name[60] ="LSJ17";
> joint_state.position [60] =-(TorsoBend/41);
> joint_state.name[61] ="LSJ18";
> joint_state.position [61] =-(TorsoBend/41);
> joint_state.name[62] ="LSJ19";
> joint_state.position [62] =-(TorsoBend/41);
> joint_state.name[63] ="LSJ20";
> joint_state.position [63] =-(TorsoBend/41);
>
>
>
> 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]));
>
>
>
>
> 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);
>
>
> joint_pub.publish(joint_state);
> broadcaster.sendTransform(odom_trans);
>
>
> }
>
>
> 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();
> }