Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Timer + Multiple Publishers run into exception #2358

Open
igricart opened this issue Oct 30, 2023 · 0 comments
Open

Timer + Multiple Publishers run into exception #2358

igricart opened this issue Oct 30, 2023 · 0 comments

Comments

@igricart
Copy link

Description

I have a node that publishes 2 topics and a timer to publish TF using ros-noetic. However this setup throws an exception:

libpthread.so.0!__GI___pthread_mutex_lock(pthread_mutex_t * mutex) (/build/glibc-BHL3KM/glibc-2.31/nptl/pthread_mutex_lock.c:67)
libroscpp.so!ros::Publication::processPublishQueue() (Unknown Source:0)
libroscpp.so!ros::TopicManager::processPublishQueues() (Unknown Source:0)
libroscpp.so!boost::signals2::detail::signal_impl<void (), boost::signals2::optional_last_value<void>, int, std::less<int>, boost::function<void ()>, boost::function<void (boost::signals2::connection const&)>, boost::signals2::mutex>::operator()() (Unknown Source:0)
libroscpp.so!ros::PollManager::threadFunc() (Unknown Source:0)
libboost_thread.so.1.71.0![Unknown/Just-In-Time compiled code] (Unknown Source:0)
libpthread.so.0!start_thread(void * arg) (/build/glibc-BHL3KM/glibc-2.31/nptl/pthread_create.c:477)
libc.so.6!clone() (/build/glibc-BHL3KM/glibc-2.31/sysdeps/unix/sysv/linux/x86_64/clone.S:95)

That's the output of the callback stack when it throws the exception.

My code:

Node

#include <ros/ros.h>
#include <calibrate_lidars/crash.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "crash");
  calibration::Crash cal;
  ros::AsyncSpinner spinner(0);
  spinner.start();
  ros::waitForShutdown();
  return 0;
}

Header file

#pragma once

#include <ros/ros.h>

#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_datatypes.h>
#include <tf2_ros/buffer.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

namespace calibration
{
class Crash
{
public:
  // Constructor
  Crash();

private:
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;
  geometry_msgs::TransformStamped tf_nominal_to_operational_;
  geometry_msgs::TransformStamped tf_nominal_to_calibrated_;
  tf2_ros::TransformBroadcaster tf_broadcaster_;
  ros::Timer tf_timer_;
  ros::Publisher pub_1_;
  ros::Publisher pub_2_;

  void initializeTfs();
};

}  // namespace calibration

Definition of methods

#include <calibrate_lidars/crash.h>

namespace calibration
{
Crash::Crash() : tf_listener_(tf_buffer_)
{
  initializeTfs();
  pub_1_ = ros::NodeHandle().advertise<geometry_msgs::TransformStamped>("topic_1", 1, false);
  pub_2_ = ros::NodeHandle().advertise<geometry_msgs::TransformStamped>("topic_2", 1, false);
}

void Crash::initializeTfs()
{
  // initializing an identity transformation from nominal to operational frame
  tf_nominal_to_operational_.header.frame_id = "lidar_nominal_frame";
  tf_nominal_to_operational_.child_frame_id = "lidar_operational_frame";
  tf_nominal_to_operational_.transform.rotation.w = 1;
  tf_nominal_to_calibrated_ = tf_nominal_to_operational_;
  tf_nominal_to_calibrated_.child_frame_id = "lidar_calibrated_frame";
  ROS_INFO_STREAM("Init transformation from nominal to operational frame: " << tf_nominal_to_operational_);
  tf_timer_ = ros::NodeHandle().createTimer(ros::Duration(0.1),
                                            [&](const ros::TimerEvent& event)
                                            {
                                              tf_nominal_to_operational_.header.stamp = ros::Time::now();
                                              tf_nominal_to_calibrated_.header.stamp = ros::Time::now();
                                              ROS_DEBUG_THROTTLE_NAMED(30, "calibration", "Sending Transforms");
                                              tf_broadcaster_.sendTransform(tf_nominal_to_operational_);
                                              tf_broadcaster_.sendTransform(tf_nominal_to_calibrated_);
                                            });
}
}  // namespace calibration

I also tried using a customized callbackQueue, but it didn't solve it.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant