Segfault in treeidsolver_recursive_newton_euler.cpp

[DimitriEckert137]

@smits

Hi everyone

This error might be due to a bad installation (which i mentioned in this issue which i am waiting for an answer to) bu I think its a different problem. I am working with a simulated ur3e robot. I use the kdl_parser to to get a kdl tree from the urdf file.

Its a segfault and by adding printf statement to treeidsolver_recursive_newton_euler.cpp I could narrow it down, so I am pretty sure it happens on the following line: const std::string& parname = GetTreeElementParent(segment->second)->first; when it passes it the SECOND time:

Thread 8 "continuous_offs" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffdeffd700 (LWP 12010)]
0x00007ffff7bb830c in std::_Rb_tree_const_iterator<std::pair<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const, KDL::TreeElement> >::operator->() const () from /home/dimitri/catkin_ws/devel/lib/libcontinuous_offset_calib.so

I tried to printf the name of the current segment to see at which segment the algorithm is at that moment but this also leads to a segfault (already at the first time it passes it).

Thread 8 "continuous_offs" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffe6ffd700 (LWP 13207)]
0x00007ffff683f470 in std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >::c_str() const ()
   from /usr/lib/x86_64-linux-gnu/libstdc++.so.6

This is my code:

ContinuousOffsetCalib.hpp:

#pragma once

#include <any_node/any_node.hpp>
#include <UrKdl.hpp>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <sstream>
#include <memory>    // for std::shared_ptr

namespace continuous_offset_calib {

class ContinuousOffsetCalib : public any_node::Node {

public:

  ContinuousOffsetCalib() = delete;
  explicit ContinuousOffsetCalib(any_node::Node::NodeHandlePtr nh) : any_node::Node(nh) {}
  ~ContinuousOffsetCalib() override = default;

  bool init() override;
  void cleanup() override;
  bool update(const any_worker::WorkerEvent& event);

  void jointStatesSubscriberCallback(const boost::shared_ptr<const sensor_msgs::JointState>& msg);
  void rokuSubscriberCallback(const boost::shared_ptr<const geometry_msgs::WrenchStamped>& msg);

  void preCleanup() override;

  ros::Subscriber joint_states_subscriber;
  ros::Subscriber roku_subscriber;
  ros::Publisher filtered_FT_publisher;

private:

  ur_kdl::UrKdl complete_robot_tree;

  KDL::JntArray current_joint_positions;            // current joint positions              

  KDL::JntArray current_joint_velocities;            // current joint positions              

  struct joint_torques {
    double elbow_joint;
    double shoulder_lift_joint;
    double shoulder_pan_joint;
    double wrist_1_joint;
    double wrist_2_joint;
    double wrist_3_joint;
  } current_joint_torques;

  unsigned int nr_joints = 6;

  KDL::JntArray tau;

  std::map<std::string,KDL::Wrench> current_wrench_FTsensor;

};

} // namespace continuous_offset_calib

ContinuousOffsetCalib.cpp:

#include <ContinuousOffsetCalib.hpp>

namespace continuous_offset_calib {

bool ContinuousOffsetCalib::init(){
  // called on startup
  // add new workers (threads) like this. The workers are automatically stopped before the cleanup() function is called
  constexpr double defaultWorkerTimeStep = 3.0;
  constexpr int priority = 10;
  auto workerTimeStep = param<double>("time_step", defaultWorkerTimeStep);
  addWorker("continuousOffsetCalib::updateWorker", workerTimeStep, &ContinuousOffsetCalib::update, this, priority);

  // to create publishers/subscribers/service clients/servers or read params, use the functions provided by the any_node::Node base class
  // the name of the publishers/subscribers/.. has to be consistent
  // with the names specified in the yaml file (see description of Topic.hpp above)
  constexpr unsigned int defaultQueueSize = 1;

  joint_states_subscriber =
    subscribe("joint_states_subscriber", "/joint_states", defaultQueueSize, &ContinuousOffsetCalib::jointStatesSubscriberCallback, this);
  roku_subscriber =
    subscribe("roku_subscriber", "/roku/FT", defaultQueueSize, &ContinuousOffsetCalib::rokuSubscriberCallback, this);

  MELO_INFO("init called");

  MELO_INFO("init tau before: %i", tau.rows());
  tau.resize(6);
  MELO_INFO("init tau after: %i", tau.rows());

  return true;
}

void ContinuousOffsetCalib::cleanup() {
  // this function is called when the node is requested to shut down, _after_ the ros spinners and workers were stopped
  // no need to stop workers which are started with addWorker(..) function
  MELO_INFO("cleanup called");
}

bool ContinuousOffsetCalib::update(const any_worker::WorkerEvent& event) {
  // called by the worker which is automatically set up if rosparam standalone == True.
  // The frequency is defined in the time_step rosparam.
  constexpr unsigned int defaultQueueSize = 1;
  filtered_FT_publisher = advertise<sensor_msgs::JointState>("filtered_FT_publisher", "/filtered_FT", defaultQueueSize);  

  current_joint_positions.resize(6);
  current_joint_velocities.resize(6);
  //tau.resize(6);

  //TODO: joint accelerations
  sensor_msgs::JointState current_estimated_torques = complete_robot_tree.calculateTorques(current_joint_positions, current_joint_velocities, current_joint_positions, current_wrench_FTsensor, tau);

  filtered_FT_publisher.publish(current_estimated_torques);

  MELO_INFO("update called");
  return true;
}

void ContinuousOffsetCalib::preCleanup() {
  // this function is called when the node is requested to shut down, _before_ the ros spinners and workers are beeing stopped
  MELO_INFO("preCleanup called");
}

void ContinuousOffsetCalib::jointStatesSubscriberCallback(const boost::shared_ptr<const sensor_msgs::JointState>& msg) {
  // called asynchrounously when ros messages arrive for the subscriber created in init() function

  current_joint_torques.elbow_joint = msg->effort[0]; 
  current_joint_torques.shoulder_lift_joint = msg->effort[1]; 
  current_joint_torques.shoulder_pan_joint = msg->effort[2]; 
  current_joint_torques.wrist_1_joint = msg->effort[3]; 
  current_joint_torques.wrist_2_joint = msg->effort[4]; 
  current_joint_torques.wrist_3_joint = msg->effort[5]; 

  current_joint_positions.data <<  msg->position[1], msg->position[2], msg->position[0], msg->position[3], msg->position[4], msg->position[5];

  current_joint_velocities.data <<  msg->velocity[1], msg->velocity[2], msg->velocity[0], msg->velocity[3], msg->velocity[4], msg->velocity[5];
}

void ContinuousOffsetCalib::rokuSubscriberCallback(const boost::shared_ptr<const geometry_msgs::WrenchStamped>& msg){

  KDL::Wrench wrench;
  wrench.force[0] = msg->wrench.force.x;
  wrench.force[1] = msg->wrench.force.y;
  wrench.force[2] = msg->wrench.force.z;

  wrench.torque[0] = msg->wrench.force.x;
  wrench.torque[1] = msg->wrench.force.y;
  wrench.torque[2] = msg->wrench.force.z;

  current_wrench_FTsensor.insert(std::pair<std::string,KDL::Wrench>((std::string)"sensor_top_link",wrench));

}

} // namespace continuous_offset_calib

UrKdl.hpp:

#pragma once

#include <kdl_parser/kdl_parser.hpp>
#include <urdf/model.h>
#include <kdl/treeidsolver_recursive_newton_euler.hpp>
#include <kdl/jacobian.hpp>
#include <kdl/tree.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/treejnttojacsolver.hpp>
#include <sensor_msgs/JointState.h>
#include <vector>

namespace ur_kdl {

class UrKdl {

public:

  UrKdl(){
    KDL::Tree ur_tree;
    ros::NodeHandle node;
    std::string robot_desc_string;
    KDL::Vector g(0.0, 0.0, 9.81);
    node.param("robot_description", robot_desc_string, std::string());
    if (!kdl_parser::treeFromString(robot_desc_string, ur_tree)){
      ROS_ERROR("Failed to construct kdl tree");
    }
    else {
      ROS_INFO("Successfully kdl_parsed urdf file");
    }
    invDynSolver = new KDL::TreeIdSolver_RNE(ur_tree, g);
  }

  ~UrKdl(){};

  // parsing kdl tree from the ros param server (only works when master and robot acturally launched not standalone) https://wiki.ros.org/kdl_parser/Tutorials/Start%20using%20the%20KDL%20parser

  sensor_msgs::JointState calculateTorques(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const KDL::WrenchMap& f_ext, KDL::JntArray &tau){
    int error = 0;
    MELO_INFO("before carttojnt tau: %i", tau.rows());
    error = invDynSolver->CartToJnt(q, q_dot, q_dotdot, f_ext, tau);
    MELO_INFO("after carttojnt tau: %i", tau.rows());
    sensor_msgs::JointState torques;
    std::vector<double> effort;
    MELO_INFO("calculateTorques, %i", error);
    for(int i = 0; i < tau.data.rows(); i++){
      effort.push_back(tau.data[i]);
      MELO_INFO("%i   %f", i, tau.data[i]);
    }
    torques.effort = effort;
    return torques;
  }

private:

  KDL::WrenchMap current_wrench_FTsensor;
  KDL::TreeIdSolver_RNE* invDynSolver;

};

} // namespace ur_kdl

[MatthijsBurgh]

Can you please split the line:

const std::string& parname = GetTreeElementParent(segment->second)->first;

into seperate lines. So we know which part of this line is the problem. (Maybe add a print inbetween each line)

Can you tell me if the library is compiled with KDL_USE_NEW_TREE_INTERFACE on or off? If you don't set it yourself. Please check CMakecache or similar to be really sure.

[DimitriEckert137]

Ok i split it up like this:

      const SegmentMap::const_iterator& parentseg =  GetTreeElementParent(segment->second);
      const std::string& parname = parentseg->first;

and it seems like the error occurs in the second line.

For the CMakeCache: I dont find this file. Which is probably due to the installation problems I mentioned above (here again the link to the github issue). So this might be the problem after all.

[MatthijsBurgh]

CMakeCache is in the build folder (catkin_ws/build/orocos_kdl).

So have you compiled kdl from source or are you using ros-$ROS_DISTRO-orocos-kdl??

It is very logical that the second line can fail, as it is an iterator, which can point to the end of an iterable. In that case it is an nullptr. So could you add a check?

if (parentseg)
    std::cout << "Iterator is valid" << std::endl;
else
  std::cout << "iterator is invalid" << std::endl;

[MatthijsBurgh]

It has not been concluded yet if this bug is related to your implementation or should be fixed in the library.

[DimitriEckert137]

It has not been concluded yet if this bug is related to your implementation or should be fixed in the library.

ok. I just closed it because i dont follow this approach anymore and use the chain inverse dynamics solver instead.

[francofusco]

Hey! I'm the author of the solver, sorry for not having seen this issue before... 😞

My best guess is that the problem is you build the KDL::Tree instance as a temporary in the constructor of UrKdl. The ID solver stores a reference to a tree object, see here. I imagine that after construction of the UrKdl object, the tree instance is destroyed and you get undefined behavior. Could you try to add the tree as a member variable of UrKdl and see if this solves the issue? If not, I'll dig more into the issue and try to find the bug :+1: