Ask Your Question
0

Warehouseviewer on real Care-o-bot hanging up

asked 2012-03-01 02:24:41 -0500

dong gravatar image

updated 2012-03-01 22:04:44 -0500

Hi, i am current working with the arm_navigation stack on our care-o-bot. I have followed the IROS ompl tutorial to run the arm navigation. I have been succeeded to send the arm command from warehouse viewer to the gazebo simulation. But when i switch to the real robot, the warehouse viewer tend to died after i push the accept button.

The arm_navigation seems to work fine, it succeeded to connect to the right action server and i got the output similar like the one i got in simulation. But the warehouse viewer doesn't seem to work. I also try to run both arm navigation and warehouseviewer on the robot and on the desktop pc or one on remote one on robot pc by changing the corresponding warehouse_hostname parameter, but it gives no improvements.

And when i run the warehouse at a desktop pc, which means the warehouse hostname is the robot pc. I got following debug info from the rxconsole. It seems to be a connection error to the database.

Starting mongodb with db location arm_navigation_dbs/cob3_4 listening on cob3-4-pc1:27020


Connection::drop(2)

TCP socket [36] closed

Connection::drop(0)

Connection::drop(2)

Connection::drop(2)

Root node has meshes 0xb238d1a0

Setting joint origin for torso_lower_neck_tilt_link to 0.155 0 0.892

Setting joint origin for torso_pan_link to 0 0 0

Root node has meshes 0x8764ff0

Setting joint origin for torso_upper_neck_tilt_link to 0 0 -0.237

Setting joint origin for head_color_camera_l_link to -0.05 -0.1 -0.01

Setting joint origin for head_cam3d_link to 0.0797163 -0.030267 0.00191677

Setting joint origin for head_cam3d_frame to 0 0 0

Setting joint origin for head_color_camera_l_frame to 0 0 0

Setting joint origin for head_color_camera_r_link to 0.120544 0.000735863 -0.00388596

Setting joint origin for head_color_camera_r_frame to 0 0 0

Root node has meshes 0x863b9c8

Setting joint origin for head_cover_link to 0 0 0

Root node has meshes 0xadc9c1d0

Setting joint origin for torso_tray_link to 0.2406 0.19 0.732

Group arm_joints

Joints:

arm_1_joint

arm_2_joint

arm_3_joint

arm_4_joint

arm_5_joint

arm_6_joint

arm_7_joint

Fixed joints:

Group links:

arm_5_link

arm_6_link

arm_3_link

arm_7_link

arm_4_link

arm_1_link

arm_2_link

Group kuka_arm

Joints:

arm_1_joint

arm_2_joint

arm_3_joint

arm_4_joint

arm_5_joint

arm_6_joint

arm_7_joint

Fixed joints:

arm_0_joint

Group links:

arm_5_link

arm_6_link

arm_3_link

arm_7_link

arm_4_link

arm_1_link

arm_2_link

arm_0_link

Calling ODE Init res 1

Initializing new thread (1 total)

Init says 1

Initializing ODE

No entry for sdh_grasp_link

No entry for sdh_tip_link

Link base_footprint padding 0.01

Link base_link padding 0.01

Link arm_0_link padding 0.01

Link arm_1_link padding 0.01

Link arm_2_link padding 0.01

Link arm_3_link padding 0.01

Link arm_4_link padding 0.01

Link arm_5_link padding 0.01

Link arm_6_link padding 0.01

Link arm_7_link padding 0.01

Link sdh_palm_link padding 0.01

Link sdh_finger_21_link padding 0.01

Link sdh_finger_22_link padding 0.01

Link sdh_finger_23_link padding 0.01

Link sdh_finger_11_link padding 0.01

Link sdh_finger_12_link padding 0.01

Link sdh_finger_13_link padding 0.01

Link sdh_thumb_1_link padding 0.01

Link sdh_thumb_2_link padding 0.01

Link sdh_thumb_3_link padding 0.01

Link ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2012-03-03 19:36:58 -0500

bit-pirate gravatar image

updated 2012-03-04 13:30:32 -0500

Hmm, that sounds a bit like my problem. As soon as I try to use real robot data the warehouse viewer crashes. Are you using an installation with the newest deb? Maybe there is a bug in that one.

Have you already tried the suggestions in this post? http://answers.ros.org/question/28163/error-when-using-real-robot-data-for

/update

I got the warehouse viewer running for me now. Maybe the below solves your problem, too.

Digging deeper using gdb I found the troublemaker in PlanningSceneEditor::jointStateCallback in move_arm_utils.cpp. In this function the assumption is made, that the velocity array of the joint_state message has the same size as the position array. This is not the case for our robot (we don't publish velocities, only positions). Hence, I changed the code as shown below and the warehouse viewer runs smoothly now.

Original:

//message already been validated in kmsm
for(unsigned int i = 0; i < joint_state->position.size(); ++i)
{
  joint_state_map[joint_state->name[i]] = joint_state->position[i];
  joint_velocity_map[joint_state->name[i]] = joint_state->velocity[i];
}

New:

//message already been validated in kmsm
if ( joint_state->velocity.size() == joint_state->position.size() )
{
  for(unsigned int i = 0; i < joint_state->position.size(); ++i)
  {
    joint_state_map[joint_state->name[i]] = joint_state->position[i];
    joint_velocity_map[joint_state->name[i]] = joint_state->velocity[i];
  }
}
else
{
  for(unsigned int i = 0; i < joint_state->position.size(); ++i)
  {
    joint_state_map[joint_state->name[i]] = joint_state->position[i];
    joint_velocity_map[joint_state->name[i]] = 0.0;
  }
}

PS: I tried attaching the patch to this post, but keep getting the error "TypeError: data is undefined".

edit flag offensive delete link more

Comments

Thanks Marcus. I have the same issue as you did. The velocity value of some joints are not published. I add some code in my driver nodes and now everything goes fine.

dong gravatar imagedong ( 2012-03-04 22:33:03 -0500 )edit

Hey guys, I just checked and my warehouse viewer still hangs after these changes. This is what my output from the console is: http://i.imgur.com/w6xMPH4.png. I am also able to visualize in rviz and I am publishing joint_states: http://i.imgur.com/y8WzBtZ.png. Any Ideas?

MartinW gravatar imageMartinW ( 2013-01-30 09:45:01 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2012-03-01 02:24:41 -0500

Seen: 489 times

Last updated: Mar 04 '12