Gazebo Actor's collisions and skeleton show up at origin

edit

Hello,

I'm using ROS Melodic (gazebo_ros) and Gazebo 9.0.0 on Ubuntu 18.04 (5.4.0-58-generic).

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 here for illustration.

Without ROS (if I run it directly in Gazebo) the plugin works fine. However, when I include it into a ROS package, the above mentioned problem appears.

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 (+ added files that are needed for a ROS package). Do I need to change anything else to avoid this issue?

Link to the same question asked on Gazebo forum: https://answers.gazebosim.org/questio...

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