Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

For my specific purpose, which is to teleport a robot, where the robot is kept fixed after teleportation, I’ve found a workaround.

The cause of segmentation fault was likely the call to physics::joint::Detach(). So I avoided calling Detach(). Then there were no more segmentation faults. (Even when I only kept one Detach() call in the entire duration of the Gazebo WorldPlugin, I still had segmentation faults from time to time. So I removed the calls entirely, and then no more seg faults. This further points to Detach() as the problem.)

I still use the Gazebo WorldPlugin, but instead of using Attach() and Detach() to let the robot loose from a fixed "world" link for teleportation, I don’t connect the robot to the world in the URDF at all. I allow the robot to float in space, and I force it still by calling SetWorldPose(teleport_pose) in the OnUpdate() function of the WorldPlugin.

The goal pose for teleportation is passed in from a custom rosservice call. Then OnUpdate() continuously sets the robot’s pose to this goal, and twist to 0 (in case of bumping into something and sliding away in 0 gravity). This effectively achieves my goal, free of seg faults.

Code snippet:

void OnUpdate (const common::UpdateInfo & /*_info*/)
{
  physics::ModelPtr model;

  // Find the model in the Gazebo world
  physics::BasePtr model_genericType = world_ -> GetByName (
    model_name);
  if (model_genericType)
    model = boost::dynamic_pointer_cast <physics::Model> (model_genericType);
  else
    fprintf (stderr, "Did not find model %s.\n", model_name.c_str ());

  if (model)
  {
    //math::Pose pose = …  // Set some goal pose to teleport to. I set this via a custom rosservice call
    model -> SetWorldTwist (math::Vector3 (), math::Vector3 ());
    model -> SetWorldPose (pose);
  }

  // Check if a rosservice call comes in
  ros::spinOnce ();
}

For my specific purpose, which is to teleport a robot, where the robot is kept fixed after teleportation, I’ve found a workaround.

The cause of segmentation fault was likely the call to physics::joint::Detach(). So I avoided calling Detach(). Then there were no more segmentation faults. (Even when I only kept one Detach() call in the entire duration of the Gazebo WorldPlugin, I still had segmentation faults from time to time. So I removed the calls entirely, and then no more seg faults. This further points to Detach() as the problem.)

I still use the Gazebo WorldPlugin, but instead of using Attach() and Detach() to let the robot loose from a fixed "world" link for teleportation, I don’t connect the robot to the world in the URDF at all. I allow the robot to float in space, and I force it still by calling SetWorldPose(teleport_pose) in the OnUpdate() function of the WorldPlugin.

The goal pose for teleportation is passed in from a custom rosservice call. Then OnUpdate() continuously sets the robot’s pose to this goal, and twist to 0 (in case of bumping into something and sliding away in 0 gravity). This effectively achieves my goal, free of seg faults.

Code snippet:

void OnUpdate (const common::UpdateInfo & /*_info*/)
{
  physics::ModelPtr model;

  // Find the model in the Gazebo world
  physics::BasePtr model_genericType = world_ -> GetByName (
    model_name);
  if (model_genericType)
    model = boost::dynamic_pointer_cast <physics::Model> (model_genericType);
  else
    fprintf (stderr, "Did not find model %s.\n", model_name.c_str ());

  if (model)
  {
    //math::Pose pose = ...  // Set some goal pose to teleport to. I set this via a custom rosservice call
    model -> SetWorldTwist (math::Vector3 (), math::Vector3 ());
    model -> SetWorldPose (pose);
  }

  // Check if a rosservice call comes in
  ros::spinOnce ();
}

For my specific purpose, which is to teleport a robot, where the robot is kept fixed after teleportation, I’ve found a workaround.

The cause of segmentation fault was likely the call to physics::joint::Detach(). So I avoided calling Detach(). Then there were no more segmentation faults. (Even when I only kept one Detach() call in the entire duration of the Gazebo WorldPlugin, I still had segmentation faults from time to time. So I removed the calls entirely, and then no more seg faults. This further points to Detach() as the problem.)

I still use the Gazebo WorldPlugin, but instead of using Attach() and Detach() to let the robot loose from a fixed "world" link for teleportation, I don’t connect the robot to the world in the URDF at all. I allow the robot to float in space, and I force it still by calling SetWorldPose(teleport_pose) in the OnUpdate() function of the WorldPlugin.

The goal pose for teleportation is passed in from a custom rosservice call. Then OnUpdate() continuously sets the robot’s pose to this goal, and twist to 0 (in case of bumping into something and sliding away in 0 gravity). This effectively achieves my goal, free of seg faults.

Code snippet:

void OnUpdate (const common::UpdateInfo & /*_info*/)
{
  physics::ModelPtr model;

  // Find the model in the Gazebo world
  physics::BasePtr model_genericType = world_ -> GetByName (
    model_name);
  if (model_genericType)
    model = boost::dynamic_pointer_cast <physics::Model> (model_genericType);
  else
    fprintf (stderr, "Did not find model %s.\n", model_name.c_str ());

  if (model)
  {
    //math::Pose pose = ...  // Set some goal pose to teleport to. I set this via a custom rosservice call
    model -> SetWorldTwist (math::Vector3 (), math::Vector3 ());
    model -> SetWorldPose (pose);
  }

  // Check if a rosservice call comes in
  ros::spinOnce ();
}

void Load (physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
  // Check ROS is initialized, subscribe to rosservice, etc. ...

  world_ = _world;
  this -> updateConnection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind (&HandWorldPlugin::OnUpdate, this, _1));
}

For my specific purpose, which is to teleport a robot, where the robot is kept fixed after teleportation, I’ve found a workaround.

The cause of segmentation fault was likely the call to physics::joint::Detach(). So I avoided calling Detach(). Then there were no more segmentation faults. (Even when I only kept one Detach() call in the entire duration of the Gazebo WorldPlugin, I still had segmentation faults from time to time. So I removed the calls entirely, and then no more seg faults. This further points to Detach() as the problem.)

I still use the Gazebo WorldPlugin, but instead of using Attach() and Detach() to let the robot loose from a fixed "world" link for teleportation, I don’t connect the robot to the world in the URDF at all. I allow the robot to float in space, and I force it still by calling SetWorldPose(teleport_pose) in the OnUpdate() function of the WorldPlugin.

The goal pose for teleportation is passed in from a custom rosservice call. Then OnUpdate() continuously sets the robot’s pose to this goal, and twist to 0 (in case of bumping into something and sliding away in 0 gravity). This effectively achieves my goal, free of seg faults.

Code snippet:

void OnUpdate (const common::UpdateInfo & /*_info*/)
{
  physics::ModelPtr model;

  // Find the model in the Gazebo world
  physics::BasePtr model_genericType = world_ -> GetByName (
    model_name);
  if (model_genericType)
    model = boost::dynamic_pointer_cast <physics::Model> (model_genericType);
  else
    fprintf (stderr, "Did not find model %s.\n", model_name.c_str ());

  if (model)
  {
    //math::Pose pose = ...  // Set some goal pose to teleport to. I set this via a custom rosservice call
    model -> SetWorldTwist (math::Vector3 (), math::Vector3 ());
    model -> SetWorldPose (pose);
  }

  // Check if a rosservice call comes in
  ros::spinOnce ();
}

void Load (physics::WorldPtr _world, sdf::ElementPtr _sdf)
{
  // Check ROS is initialized, subscribe to rosservice, etc. ...

  world_ = _world;
  this -> updateConnection_ = event::Events::ConnectWorldUpdateBegin(
      boost::bind (&HandWorldPlugin::OnUpdate, (&MyWorldPlugin::OnUpdate, this, _1));
}