arm_kinematics_constraint_aware dies after call to get_fk

asked 2012-09-19 02:37:37 -0600

3060 ●32 ●57 ●83 http://people.idsia.ch/~gromov

updated 2012-09-19 21:02:16 -0600

I am using autogenerated custom lwa_arm_navigation stack acquired from Planning Description Configuration Wizard on Fuerte. Arm is the 7 DOF Schunk LWA provided within care-o-bot stack.

When calling /lwa_arm_kinematics/get_fk, a node adopted from this tutorial returns error:

Forward kinematics service call failed

and arm_kinematics_constraint_aware node dies with the following error:

[lwa_arm_kinematics-5] process has died [pid 12417, exit code -11, cmd /home/0xff/ros/arm_navigation/arm_kinematics_constraint_aware/bin/arm_kinematics_constraint_aware __name:=lwa_arm_kinematics __log:=/home/0xff/.ros/log/0f7bc99c-024c-11e2-8d0d-c860005ffb62/lwa_arm_kinematics-5.log].
log file: /home/0xff/.ros/log/0f7bc99c-024c-11e2-8d0d-c860005ffb62/lwa_arm_kinematics-5*.log

Although this problem looks exactly same as on Electric provided solution does not work anymore. So is it a problem of different nature in my case?

Request to get_fk is filled as:

fk_request.header.frame_id = "base_link";
fk_request.fk_link_names.push_back("arm_7_link");
for(unsigned int i=0; i < response.kinematic_solver_info.joint_names.size(); i++)
{
  fk_request.robot_state.joint_state.position[i] = 0.0;
}

The kinematic chain is:

$ rosrun urdf_parser check_urdf lwa.urdf
robot name is: lwa
---------- Successfully Parsed XML ---------------
root Link: base_link has 1 child(ren)
    child(1):  arm_0_link
        child(1):  arm_1_link
            child(1):  arm_2_link
                child(1):  arm_3_link
                    child(1):  arm_4_link
                        child(1):  arm_5_link
                            child(1):  arm_6_link
                                child(1):  arm_7_link

However /lwa_arm_kinematics/get_fk_solver_info saying there is only one link -- arm_7_link

Thanks in advance for any help

EDIT#1:

Here is the output from gdb

Program received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffe0fce700 (LWP 27607)]
0x00007ffff7ddd39a in std_msgs::Header_<std::allocator<void> >::operator=(std_msgs::Header_<std::allocator<void> > const&) ()
   from /opt/ros/fuerte/lib/libsensor_msgs.so
Missing separate debuginfos, use: debuginfo-install
<--- a lot of package names here --->
(gdb) continue 
Continuing.
[Thread 0x7fffcb7fe700 (LWP 27647) exited]
[Thread 0x7fffcbfff700 (LWP 27639) exited]
[Thread 0x7fffe0fce700 (LWP 27607) exited]
[Thread 0x7fffe17cf700 (LWP 27606) exited]
[Thread 0x7fffe23db700 (LWP 27596) exited]
[Thread 0x7fffe2bdc700 (LWP 27595) exited]
[Thread 0x7fffe33dd700 (LWP 27594) exited]
Program terminated with signal SIGSEGV, Segmentation fault.
The program no longer exists.

EDIT#2:

I am not good at debugging with plain gdb so I did it with the help of Eclipse. So the cause of SIGSEGV is here:

bool
ArmKinematicsConstraintAware::getPositionFK(
                 kinematics_msgs::GetPositionFK::Request &request,
                 kinematics_msgs::GetPositionFK::Response &response){
// --- cut --- //
  if(!collision_models_interface_->convertPoseGivenWorldTransform(
                           *state, request.header.frame_id, world_header,
                           poses[i], response.pose_stamped[i])) {
    response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
    return true;
  }
// --- cut --- //
}

Here poses and response.pose_stamped has different size, while the counter runs based on poses.size() which is 1, the response.pose_stamped is empty.

Now, the question is it a bug or I should initialize response fields by myself?

edit retag flag offensive close merge delete

Comments

Could you investigate a bit more, when exactly (at which line) the node dies? You can use GDB via roslaunch to do a backtrace.

bit-pirate ( 2012-09-19 13:54:22 -0600 )edit

Are the joint names filled in the robot_state in the request? (nevertheless, the node shouldn't abort)

dornhege ( 2012-09-19 23:10:46 -0600 )edit

@dornhege, yes, same as in tutorial: fk_request.robot_state.joint_state.name = response.kinematic_solver_info.joint_names;

Boris ( 2012-09-20 00:42:59 -0600 )edit

add a comment

2

answered 2012-09-20 01:08:39 -0600

Boris

3060 ●32 ●57 ●83 http://people.idsia.ch/~gromov

updated 2012-09-20 20:02:07 -0600

I am still not sure is it a bug or not, but with this patch it works

--- a/arm_kinematics_constraint_aware/src/arm_kinematics_constraint_aware.cpp    2012-09-20 19:57:27.443296318 +0900
+++ b/arm_kinematics_constraint_aware/src/arm_kinematics_constraint_aware.cpp   2012-09-20 19:58:20.796409267 +0900
@@ -339,6 +339,8 @@ bool ArmKinematicsConstraintAware::getPo
                                       request.fk_link_names,
                                       poses);
   if(valid) {
+    response.pose_stamped.resize(request.fk_link_names.size());
+    response.fk_link_names.resize(request.fk_link_names.size());
     for(unsigned int i=0; i < poses.size(); i++) { 

       std_msgs::Header world_header;
       world_header.frame_id = collision_models_interface_->getWorldFrameId();

EDIT #1:

Opened a ticket for the bug.

edit flag offensive delete link

Comments

First, if the above works for you, great! However, I have the feeling this patch is not the right way to solve your problem. I took a look at the code snippets you mentioned and indeed couldn't find a line, which adjusts the size of pose_stamped. You say, the node crashes ...

bit-pirate ( 2012-09-20 13:48:26 -0600 )edit

in the call to "convertPoseGivenWorldTransform()". Could you find out at which line in this method the node dies? If in the end, we find no other cause of this crash and an according fix, I recommend sending in your patch via a bug ticket.

bit-pirate ( 2012-09-20 13:55:09 -0600 )edit

It dies on the first line inside convertPoseGivenWorldTransform() during assignment of ret_pose.header = header. That's may be because there is no boundary check on STL vectors when assign with index.

Boris ( 2012-09-20 17:11:48 -0600 )edit

add a comment

0

answered 2014-06-30 08:35:19 -0600