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