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
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();
this->line_color = _sdf->GetElement("color")->Get();
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.
↧