Actor's collisions and skeleton show up at origin

edit

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/3713...

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 ...