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: ------Original State Publisher---------------- #include #include #include #include #include #include int main(int argc, char** argv) { ros::init(argc, argv, "intial_state_publisher"); ros::NodeHandle n; ros::Publisher joint_pub = n.advertise("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 #include #include #include using namespace std; using namespace ros; void jointCallback(const sensor_msgs::JointStateConstPtr& state) { ros::NodeHandle n; ros::Publisher joint_pub = n.advertise("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 joint_positions; for (unsigned int i=0; iname.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(); }