Robotics StackExchange | Archived questions

Segmentation Fault in Gazebo Visual Plugin with ROS.

I'm currently trying to show dynamic lines in gazebo by Visual Plugin using topic data from ROS. However, I frequently get a segmentation fault error in gazebo when I am running the visual plugin.

My environment is:

・Ubuntu 14.04.3  

・ROS indigo

・gazebo 2.2

Here is the error message.

Segmentation fault (core dumped)
[gazebo_gui-2] process has died [pid 18242, exit code 139, cmd /opt/ros/indigo/lib/gazebo_ros/gzclient    __name:=gazebo_gui __log:=/home/ken/.ros/log/119a27d2-ca49-11e5-89c2-d0509984724f/gazebo_gui-2.log].
log file: /home/ken/.ros/log/119a27d2-ca49-11e5-89c2-d0509984724f/gazebo_gui-2*.log
Segmentation fault (core dumped)
[gazebo-1] process has died [pid 18238, exit code 139, cmd /opt/ros/indigo/lib/gazebo_ros/gzserver -e ode/home/ken/catkin_ws/src/navigation/worlds/maze.world __name:=gazebo __log:=/home/ken/.ros/log/119a27d2-ca49-11e5-89c2-d0509984724f/gazebo-1.log].
log file: /home/ken/.ros/log/119a27d2-ca49-11e5-89c2-d0509984724f/gazebo-1*.log

Here is my source code.

#include <navigation/trajectoryline.h>

namespace gazebo
{
  namespace rendering
  {

////////////////////////////////////////////////////////////////////////////////                                                                                                                                                                                                                                                                                                                                           
// Constructor                                                                                                                                                                                                                                                                                                                                                                                                             
TrajectoryPlugin::TrajectoryPlugin()

{
  line=NULL;
}

////////////////////////////////////////////////////////////////////////////////                                                                                                                                                                                                                                                                                                                                           
// Destructor                                                                                                                                                                                                                                                                                                                                                                                                              
TrajectoryPlugin::~TrajectoryPlugin()
{
  // Finalize the visualizer                                                                                                                                                                                                                                                                                                                                                                                               
  this->rosnode_->shutdown();
  delete this->rosnode_;
}

////////////////////////////////////////////////////////////////////////////////                                                                                                                                                                                                                                                                                                                                           
// Load the plugin                                                                                                                                                                                                                                                                                                                                                                                                         
void TrajectoryPlugin::Load( VisualPtr _parent, sdf::ElementPtr _sdf )
{
  this->visual_ = _parent;

  this->visual_namespace_ = "visual/";

  // start ros node                                                                                                                                                                                                                                                                                                                                                                                                        
  if (!ros::isInitialized())
    {
      int argc = 0;
      char** argv = NULL;
      ros::init(argc,argv,"gazebo_visual",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
    }
  std::string robot_name = _sdf->GetElement("robot_name")->Get<std::string>();
  this->line_color = _sdf->GetElement("color")->Get<std::string>();
  this->rosnode_ = new ros::NodeHandle;//(this->visited_visual_namespace_);                                                                                                                                                                                                                                                                                                                                                
  this->trajectory_sub_ = this->rosnode_->subscribe("/"+robot_name+"/trajectory", 1, &TrajectoryPlugin::GetTrajectory, this);
  this->visual_->DetachObjects();
  // Listen to the update event. This event is broadcast every                                                                                                                                                                                                                                                                                                                                                             
  // simulation iteration.                                                                                                                                                                                                                                                                                                                                                                                                 
  this->update_connection_ = event::Events::ConnectRender(
                                                          boost::bind(&TrajectoryPlugin::UpdateChild, this));
}

//////////////////////////////////////////////////////////////////////////////////                                                                                                                                                                                                                                                                                                                                         
// Update the visualizer                                                                                                                                                                                                                                                                                                                                                                                                   
void TrajectoryPlugin::UpdateChild()
{
  ros::spinOnce();
}

//////////////////////////////////////////////////////////////////////////////////                                                                                                                                                                                                                                                                                                                                         
// VisualizeForceOnLink                                                                                                                                                                                                                                                                                                                                                                                                    
void TrajectoryPlugin::GetTrajectory(const std_msgs::Float64MultiArray::ConstPtr  &msg)
{
  this->visual_->DetachObjects();
  this->line = this->visual_->CreateDynamicLine(RENDERING_LINE_STRIP);
  point_V relaypoints = ros_msgs::arrayMsgTopoint_V(*msg);
  for(int i = 0; i < relaypoints.size();i++){
      this->line->AddPoint(math::Vector3(relaypoints[i].x,relaypoints[i].y,relaypoints[i].z));
  }
  // set the Material of the line, in this case to purple                                                                                                                                                                                                                                                                                                                                                                  
  this->line->setMaterial("Gazebo/"+this->line_color);
    this->line->setVisibilityFlags(GZ_VISIBILITY_GUI);
  this->visual_->SetVisible(true);
}

// Register this plugin within the simulator                                                                                                                                                                                                                                                                                                                                                                               
GZ_REGISTER_VISUAL_PLUGIN(TrajectoryPlugin)
   }
}

I think the error occurs when the TrajectoryPlugin::GetTrajectory is called and after a few updates in gazebo.

Can you tell me how to get rid of this problem?

Thanks in advance.

Asked by kenkiki on 2016-02-03 04:10:46 UTC

Comments

Can you probide a gdb backtrace?

Asked by nkoenig on 2016-02-03 10:28:40 UTC

Thank you. I did gdb backtrace, but I couldn't get any clues from that.

Here's the backtrace. Program terminated with signal SIGSEGV, Segmentation fault.

0 0x00007f497b605c31 in ?? ()

(gdb) bt

0 0x00007f497b605c31 in ?? ()

1 0x00000000004ac2db in ?? ()

2 0xffff80033c0627e1 in ?? ()

3 0x000000000191d878 in ?? ()

.....

83 0x0000000000000000 in ?? ()

Asked by kenkiki on 2016-02-04 07:20:46 UTC

Answers