Hello, I have been trying to move an actor with a Joystick with ROS2 and Gazebo Sim (harmonic) however I am having difficulties. Firstly to simplify the problem I have attempted to create a custom plugin to modify the position of the actor but the attempt has been unsuccessful, only have been able to extract information for the simulation but when I try to modify the agent’s position it just does not respond.
As template for the plugin I have used this: ros_gz_project_template/ros_gz_example_gazebo at main · gazebosim/ros_gz_project_template · GitHub
This is the code of the plugin
header:
/*
* Copyright (C) 2022 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef ROS_GZ_EXAMPLE_GAZEBO__FULL_SYSTEM_HH_
#define ROS_GZ_EXAMPLE_GAZEBO__FULL_SYSTEM_HH_
// The only required include in the header is this one.
// All others will depend on what your plugin does.
#include <gz/sim/EventManager.hh>
#include <gz/sim/System.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/Actor.hh>
#include "rclcpp/rclcpp.hpp"
namespace ros_gz_example_gazebo
{
// This is the main plugin's class. It must inherit from System and at least
// one other interface.
// Here we use `ISystemPostUpdate`, which is used to get results after
// physics runs. The opposite of that, `ISystemPreUpdate`, would be used by
// plugins that want to send commands.
class FullSystem:
public gz::sim::System,
public gz::sim::ISystemConfigure,
public gz::sim::ISystemPreUpdate,
public gz::sim::ISystemUpdate,
public gz::sim::ISystemPostUpdate,
public gz::sim::ISystemReset
{
/// \brief Constructor
public: FullSystem();
// Plugins inheriting ISystemConfigure must implement the Configure
// callback. This is called when a system is initially loaded.
// The _entity variable contains the entity that the system is attached to
// The _element variable contains the sdf Element with custom configuration
// The _ecm provides an interface to all entities and components
// The _eventManager provides a mechanism for registering internal signals
public: void Configure(
const gz::sim::Entity &_entity,
const std::shared_ptr<const sdf::Element> &_element,
gz::sim::EntityComponentManager &_ecm,
gz::sim::EventManager &_eventManager) override;
// Plugins inheriting ISystemPreUpdate must implement the PreUpdate
// callback. This is called at every simulation iteration before the physics
// updates the world. The _info variable provides information such as time,
// while the _ecm provides an interface to all entities and components in
// simulation.
public: void PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) override;
// Plugins inheriting ISystemUpdate must implement the Update
// callback. This is called at every simulation iteration before the physics
// updates the world. The _info variable provides information such as time,
// while the _ecm provides an interface to all entities and components in
// simulation.
public: void Update(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) override;
// Plugins inheriting ISystemPostUpdate must implement the PostUpdate
// callback. This is called at every simulation iteration after the physics
// updates the world. The _info variable provides information such as time,
// while the _ecm provides an interface to all entities and components in
// simulation.
public: void PostUpdate(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm) override;
// Plugins inheriting ISystemReset must implement the Reset callback.
// This is called when simulation is reset/rewound to initial conditions.
public: void Reset(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm) override;
public: std::shared_ptr<rclcpp::Node> rosnode;
private: gz::math::Pose3d actorPose;
};
}
#endif
CPP script:
/*
* Copyright (C) 2022 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
// We'll use a string and the gzmsg command below for a brief example.
// Remove these includes if your plugin doesn't need them.
#include <string>
#include <gz/common/Console.hh>
// This header is required to register plugins. It's good practice to place it
// in the cc file, like it's done here.
#include <gz/plugin/Register.hh>
// Don't forget to include the plugin's header.
#include "ros_gz_example_gazebo/FullSystem.hh"
// This is required to register the plugin. Make sure the interfaces match
// what's in the header.
GZ_ADD_PLUGIN(
ros_gz_example_gazebo::FullSystem,
gz::sim::System,
ros_gz_example_gazebo::FullSystem::ISystemConfigure,
ros_gz_example_gazebo::FullSystem::ISystemPreUpdate,
ros_gz_example_gazebo::FullSystem::ISystemUpdate,
ros_gz_example_gazebo::FullSystem::ISystemPostUpdate,
ros_gz_example_gazebo::FullSystem::ISystemReset
)
namespace ros_gz_example_gazebo
{
FullSystem::FullSystem(){
// Initialize ROS if it hasn't been initialized
if (!rclcpp::ok())
{
int argc = 0;
char **argv = nullptr;
rclcpp::init(argc, argv);
}
// Create the ROS 2 Node
rosnode = std::make_shared<rclcpp::Node>("FullSystem");
// Example usage: print out a log message to verify the node was created successfully
RCLCPP_INFO(rosnode->get_logger(), "FullSystem Node has been successfully created.");
}
void FullSystem::Configure(const gz::sim::Entity &_entity,
const std::shared_ptr<const sdf::Element> &_element,
gz::sim::EntityComponentManager &_ecm,
gz::sim::EventManager &_eventManager)
{
gzdbg << "ros_gz_example_gazebo::FullSystem::Configure on entity: " << _entity << std::endl;
}
void FullSystem::PreUpdate(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm)
{
if (!_info.paused && _info.iterations % 1000 == 0)
{
gzdbg << "ros_gz_example_gazebo::FullSystem::PreUpdate" << std::endl;
}
}
void FullSystem::Update(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm)
{
if (!_info.paused && _info.iterations % 1000 == 0)
{
gzdbg << "ros_gz_example_gazebo::FullSystem::Update" << std::endl;
}
gz::sim::Entity entityActor = _ecm.EntityByComponents(gz::sim::components::Name("actor_walking"));
gz::sim::Actor actor_ = gz::sim::Actor(entityActor);
if (!_info.paused)
{
gz::math::Pose3d actorPose = actor_.WorldPose(_ecm).value();
gz::math::Vector3d positionAgent = actorPose.Pos();
gzdbg << "Actor with Pose in X axis:" << positionAgent.X() << std::endl;
RCLCPP_INFO(rosnode->get_logger(), "Human Agent with position in x-axis: %f", positionAgent.X());
actorPose.Pos().X(positionAgent.X() + 0.01);
actor_.SetTrajectoryPose(_ecm, actorPose);
}
}
void FullSystem::PostUpdate(const gz::sim::UpdateInfo &_info,
const gz::sim::EntityComponentManager &_ecm)
{
if (!_info.paused && _info.iterations % 1000 == 0)
{
gzdbg << "ros_gz_example_gazebo::FullSystem::PostUpdate" << std::endl;
}
gz::sim::Entity entityActor = _ecm.EntityByComponents(gz::sim::components::Name("actor_walking"));
gz::sim::Actor actor_ = gz::sim::Actor(entityActor);
if (!_info.paused)
{
gz::math::Pose3d actorPose = actor_.WorldPose(_ecm).value();
}
}
void FullSystem::Reset(const gz::sim::UpdateInfo &_info,
gz::sim::EntityComponentManager &_ecm)
{
gzdbg << "ros_gz_example_gazebo::FullSystem::Reset" << std::endl;
}
} // namespace ros_gz_example_gazebo
SDF file (world)
<?xml version="1.0" ?>
<!--
Try sending commands:
gz topic -t "/model/diff_drive/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 1.0}, angular: {z: -0.1}"
ros2 topic pub /diff_drive/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 5.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: -0.1}}"
Listen to odometry:
gz topic -e -t /model/diff_drive/odometry
ros2 topic echo /model/diff_drive/odometry
-->
<sdf version="1.8">
<world name="demo">
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="BasicSystem"
name="ros_gz_example_gazebo::BasicSystem">
</plugin>
<plugin
filename="FullSystem"
name="ros_gz_example_gazebo::FullSystem">
</plugin>
<light name="sun" type="directional">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
<actor name="actor_walking">
<skin>
<filename>https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
<scale>1.0</scale>
</skin>
<animation name="walk">
<filename>https://fuel.gazebosim.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
<interpolate_x>true</interpolate_x>
</animation>
<script>
<loop>true</loop>
<delay_start>0.000000</delay_start>
<auto_start>true</auto_start>
<trajectory id="0" type="walking">
<waypoint>
<time>0</time>
<pose>0 0 1.0 0 0 0</pose>
</waypoint>
<waypoint>
<time>2</time>
<pose>2.0 0 1.0 0 0 0</pose>
</waypoint>
</trajectory>
</script>
</actor>
<model name="ground_plane">
<static>true</static>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<ambient>0.8 0.8 0.8 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<model name="diff_drive">
<self_collide>true</self_collide>
<pose>10 10 0.35 0 0 0</pose>
<include merge="true">
<uri>package://ros_gz_example_description/models/diff_drive</uri>
</include>
<plugin
filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
</plugin>
<plugin
filename="gz-sim-pose-publisher-system"
name="gz::sim::systems::PosePublisher">
<publish_link_pose>true</publish_link_pose>
<use_pose_vector_msg>true</use_pose_vector_msg>
<static_publisher>true</static_publisher>
<static_update_frequency>1</static_update_frequency>
</plugin>
<plugin
filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
<odom_frame>diff_drive/odom</odom_frame>
<robot_base_frame>diff_drive</robot_base_frame>
</plugin>
</model>
</world>
</sdf>