0
$\begingroup$

Rosanswers logo

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?


Originally posted by Boris on ROS Answers with karma: 3060 on 2012-09-19

Post score: 0


Original comments

Comment by bit-pirate on 2012-09-19:
Could you investigate a bit more, when exactly (at which line) the node dies? You can use GDB via roslaunch to do a backtrace.

Comment by dornhege on 2012-09-19:
Are the joint names filled in the robot_state in the request? (nevertheless, the node shouldn't abort)

Comment by Boris on 2012-09-20:
@dornhege, yes, same as in tutorial: fk_request.robot_state.joint_state.name = response.kinematic_solver_info.joint_names;

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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.


Originally posted by Boris with karma: 3060 on 2012-09-20

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by bit-pirate on 2012-09-20:
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 ...

Comment by bit-pirate on 2012-09-20:
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.

Comment by Boris on 2012-09-20:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.