Hello,

I'm using Gazebo 9.0.0 on Ubuntu 18.04. I'm also using gazebo_ros (ROS Melodic).

I've been working with the ActorPlugin. I want the actor to publish his pose to be later detected by the logical camera plugin, so I've changed this->actor->SetWorldPose(pose, false, false); to this->actor->SetWorldPose(pose, true, true); in the ActorPlugin.cc. This however leads to the following effect: Actor's collisions and skeleton are constantly moving from the correct position to the origin of the coordinate frame and then back. See the images for illustration:

I created a ROS package. Below is the code that I'm using, almost completely copied from ActorPlugin. I've only changed the line I've mentioned above and removed some objects from the world file. Do I need to change anything else to avoid this issue?

Link to the same question asked on the ROS forum: https://answers.ros.org/question/371302/gazebo-actors-collisions-and-skeleton-show-up-at-origin/

File CmakeLists.txt:

cmake_minimum_required(VERSION 3.0.2)
project(mycafe)

find_package(catkin REQUIRED COMPONENTS
  gazebo_ros
  roscpp
)

find_package(gazebo REQUIRED)

catkin_package(
)
include_directories(
  include/mycafe
  ${catkin_INCLUDE_DIRS}
  ${GAZEBO_INCLUDE_DIRS}
)

add_library(CafeActorPlugin src/CafeActorPlugin.cc)
target_link_libraries(CafeActorPlugin ${GAZEBO_LIBRARIES})

File package.xml:

<?xml version="1.0"?>
<package format="2">
  <name>mycafe</name>
  <version>0.0.0</version>
  <description>The mycafe package</description>

  <maintainer email="gazela@todo.todo">gazela</maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>gazebo_ros</build_depend>
  <build_depend>roscpp</build_depend>
  <build_export_depend>gazebo_ros</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <exec_depend>gazebo_ros</exec_depend>
  <exec_depend>roscpp</exec_depend>

  <export>
  </export>
</package>

File launch/mycafe.launch:

<launch>
  <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" default="$(find mycafe)/worlds/mycafe.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="recording" value="false"/>
    <arg name="debug" value="false"/>
    <arg name="verbose" value="true"/>
  </include>
</launch>

File worlds/mycafe.world

<!-- Usage notes are located in this file, look for "Usage"-->
<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <include>
      <uri>model://ground_plane</uri>
    </include>

    <actor name="actor1">
      <pose>0 1 1.25 0 0 0</pose>
      <skin>
        <filename>walk.dae</filename>
        <scale>1.0</scale>
      </skin>
      <animation name="walking">
        <filename>walk.dae</filename>
        <scale>1.000000</scale>
        <interpolate_x>true</interpolate_x>
      </animation>

      <plugin name="actor1_plugin" filename="libCafeActorPlugin.so">
        <target>0 -5 1.2138</target>
        <target_weight>1.15</target_weight>
        <obstacle_weight>1.8</obstacle_weight>
        <animation_factor>5.1</animation_factor>
        <!-- Usage: Modify the set of models that the vector field should
             ignore when moving the actor -->
        <ignore_obstacles>
          <model>cafe</model>
          <model>ground_plane</model>
        </ignore_obstacles>
      </plugin>
    </actor>

  </world>
</sdf>

File src/CafeActorPlugin.cc:

#include "CafeActorPlugin.hh"

#include <functional>
#include <ignition/math.hh>

#include "gazebo/physics/physics.hh"

using namespace gazebo;
GZ_REGISTER_MODEL_PLUGIN(CafeActorPlugin)

#define WALKING_ANIMATION "walking"

/////////////////////////////////////////////////
CafeActorPlugin::CafeActorPlugin()
{
}

/////////////////////////////////////////////////
void CafeActorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
  this->sdf = _sdf;
  this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
  this->world = this->actor->GetWorld();

  this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
      std::bind(&CafeActorPlugin::OnUpdate, this, std::placeholders::_1)));

  this->Reset();

  // Read in the target weight
  if (_sdf->HasElement("target_weight"))
    this->targetWeight = _sdf->Get<double>("target_weight");
  else
    this->targetWeight = 1.15;

  // Read in the obstacle weight
  if (_sdf->HasElement("obstacle_weight"))
    this->obstacleWeight = _sdf->Get<double>("obstacle_weight");
  else
    this->obstacleWeight = 1.5;

  // Read in the animation factor (applied in the OnUpdate function).
  if (_sdf->HasElement("animation_factor"))
    this->animationFactor = _sdf->Get<double>("animation_factor");
  else
    this->animationFactor = 4.5;

  // Add our own name to models we should ignore when avoiding obstacles.
  this->ignoreModels.push_back(this->actor->GetName());

  // Read in the other obstacles to ignore
  if (_sdf->HasElement("ignore_obstacles"))
  {
    sdf::ElementPtr modelElem =
        _sdf->GetElement("ignore_obstacles")->GetElement("model");
    while (modelElem)
    {
      this->ignoreModels.push_back(modelElem->Get<std::string>());
      modelElem = modelElem->GetNextElement("model");
    }
  }
}

/////////////////////////////////////////////////
void CafeActorPlugin::Reset()
{
  this->velocity = 0.8;
  this->lastUpdate = 0;

  if (this->sdf && this->sdf->HasElement("target"))
    this->target = this->sdf->Get<ignition::math::Vector3d>("target");
  else
    this->target = ignition::math::Vector3d(0, -5, 1.2138);

  auto skelAnims = this->actor->SkeletonAnimations();
  if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
  {
    gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
  }
  else
  {
    // Create custom trajectory
    this->trajectoryInfo.reset(new physics::TrajectoryInfo());
    this->trajectoryInfo->type = WALKING_ANIMATION;
    this->trajectoryInfo->duration = 1.0;

    this->actor->SetCustomTrajectory(this->trajectoryInfo);
  }
}

/////////////////////////////////////////////////
void CafeActorPlugin::ChooseNewTarget()
{
  ignition::math::Vector3d newTarget(this->target);
  while ((newTarget - this->target).Length() < 2.0)
  {
    newTarget.X(ignition::math::Rand::DblUniform(-3, 3.5));
    newTarget.Y(ignition::math::Rand::DblUniform(-10, 2));

    for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
    {
      double dist = (this->world->ModelByIndex(i)->WorldPose().Pos() - newTarget).Length();
      if (dist < 2.0)
      {
        newTarget = this->target;
        break;
      }
    }
  }
  this->target = newTarget;
}

/////////////////////////////////////////////////
void CafeActorPlugin::HandleObstacles(ignition::math::Vector3d &_pos)
{
  for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
  {
    physics::ModelPtr model = this->world->ModelByIndex(i);
    if (std::find(this->ignoreModels.begin(), this->ignoreModels.end(),
                  model->GetName()) == this->ignoreModels.end())
    {
      ignition::math::Vector3d offset = model->WorldPose().Pos() -
                                        this->actor->WorldPose().Pos();
      double modelDist = offset.Length();
      if (modelDist < 4.0)
      {
        double invModelDist = this->obstacleWeight / modelDist;
        offset.Normalize();
        offset *= invModelDist;
        _pos -= offset;
      }
    }
  }
}

/////////////////////////////////////////////////
void CafeActorPlugin::OnUpdate(const common::UpdateInfo &_info)
{
  // Time delta
  double dt = (_info.simTime - this->lastUpdate).Double();

  ignition::math::Pose3d pose = this->actor->WorldPose();
  ignition::math::Vector3d pos = this->target - pose.Pos();
  ignition::math::Vector3d rpy = pose.Rot().Euler();

  double distance = pos.Length();

  // Choose a new target position if the actor has reached its current
  // target.
  if (distance < 0.3)
  {
    this->ChooseNewTarget();
    pos = this->target - pose.Pos();
  }

  // Normalize the direction vector, and apply the target weight
  pos = pos.Normalize() * this->targetWeight;

  // Adjust the direction vector by avoiding obstacles
  this->HandleObstacles(pos);

  // Compute the yaw orientation
  ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
  yaw.Normalize();

  // Rotate in place, instead of jumping.
  if (std::abs(yaw.Radian()) > IGN_DTOR(10))
  {
    pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z() + yaw.Radian() * 0.001);
  }
  else
  {
    pose.Pos() += pos * this->velocity * dt;
    pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z() + yaw.Radian());
  }

  // Make sure the actor stays within bounds
  pose.Pos().X(std::max(-3.0, std::min(3.5, pose.Pos().X())));
  pose.Pos().Y(std::max(-10.0, std::min(2.0, pose.Pos().Y())));
  pose.Pos().Z(1.2138);

  // Distance traveled is used to coordinate motion with the walking
  // animation
  double distanceTraveled = (pose.Pos() -
                             this->actor->WorldPose().Pos())
                                .Length();

  this->actor->SetWorldPose(pose, true, true);
  this->actor->SetScriptTime(this->actor->ScriptTime() +
                             (distanceTraveled * this->animationFactor));
  this->lastUpdate = _info.simTime;
}

File include/mycafe/CafeActorPlugin.hh:

#ifndef GAZEBO_PLUGINS_CAFEACTORPLUGIN_HH_
#define GAZEBO_PLUGINS_CAFEACTORPLUGIN_HH_

#include <string>
#include <vector>

#include "gazebo/common/Plugin.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  class GZ_PLUGIN_VISIBLE CafeActorPlugin : public ModelPlugin
  {
    /// \brief Constructor
  public:
    CafeActorPlugin();

    /// \brief Load the actor plugin.
    /// \param[in] _model Pointer to the parent model.
    /// \param[in] _sdf Pointer to the plugin's SDF elements.
  public:
    virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

    // Documentation Inherited.
  public:
    virtual void Reset();

    /// \brief Function that is called every update cycle.
    /// \param[in] _info Timing information
  private:
    void OnUpdate(const common::UpdateInfo &_info);

    /// \brief Helper function to choose a new target location
  private:
    void ChooseNewTarget();

    /// \brief Helper function to avoid obstacles. This implements a very
    /// simple vector-field algorithm.
    /// \param[in] _pos Direction vector that should be adjusted according
    /// to nearby obstacles.
  private:
    void HandleObstacles(ignition::math::Vector3d &_pos);

    /// \brief Pointer to the parent actor.
  private:
    physics::ActorPtr actor;

    /// \brief Pointer to the world, for convenience.
  private:
    physics::WorldPtr world;

    /// \brief Pointer to the sdf element.
  private:
    sdf::ElementPtr sdf;

    /// \brief Velocity of the actor
  private:
    ignition::math::Vector3d velocity;

    /// \brief List of connections
  private:
    std::vector<event::ConnectionPtr> connections;

    /// \brief Current target location
  private:
    ignition::math::Vector3d target;

    /// \brief Target location weight (used for vector field)
  private:
    double targetWeight = 1.0;

    /// \brief Obstacle weight (used for vector field)
  private:
    double obstacleWeight = 1.0;

    /// \brief Time scaling factor. Used to coordinate translational motion
    /// with the actor's walking animation.
  private:
    double animationFactor = 1.0;

    /// \brief Time of the last update.
  private:
    common::Time lastUpdate;

    /// \brief List of models to ignore. Used for vector field
  private:
    std::vector<std::string> ignoreModels;

    /// \brief Custom trajectory info.
  private:
    physics::TrajectoryInfoPtr trajectoryInfo;
  };
} // namespace gazebo
#endif