yenon/ROS_Workspace
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Introduce new Actor functionality

Browse Source
This commit is contained in:
Bastian Hofmann 2022-12-07 10:07:15 +00:00
parent 42e03ac848
commit 9ecac8db13
4 changed files with 287 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/gaz_simulation/world/gaz_ign.sdf
View File

@ -243,6 +243,18 @@
</link>
</model>
<actor name="actor_walking">
<pose>0 2 1.25 0 0 0</pose>
<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>
</actor>
<actor name="actor1">
<pose>0 1 1.25 0 0 0</pose>
@ -250,6 +262,11 @@
<filename>/home/ros/go.dae</filename>
<scale>1.0</scale>
</skin>
<animation name="walking">
<filename>/home/ros/go.dae</filename>
<scale>1.000000</scale>
<interpolate_x>true</interpolate_x>
</animation>
<plugin name="ActorSystem" filename="/home/ros/workspace/build/ign_actor_plugin/libActorPlugin.so">
<target>2 2 1</target>
<target_weight>1.15</target_weight>

82
src/gaz_simulation/world/gaz_new_test.sdf Normal file
View File

@ -0,0 +1,82 @@
<sdf version="1.6">
<world name="empty">
<physics name="1ms" type="ignored">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="gz-sim-physics-system"
name="gz::sim::systems::Physics">
</plugin>
<plugin
filename="gz-sim-user-commands-system"
name="gz::sim::systems::UserCommands">
</plugin>
<plugin
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="gz-sim-contact-system"
name="gz::sim::systems::Contact">
</plugin>
<light type="directional" name="sun">
<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">
<plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libActorPlugin.so">
</plugin>
<skin>
<filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
<scale>1.0</scale>
</skin>
<animation name='walk'>
<filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae</filename>
</animation>
<animation name='go'>
<filename>/home/ros/go.dae</filename>
</animation>
</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>
</world>
</sdf>

171
src/ign_actor_plugin/src/ActorSystem.cpp
View File

@ -2,43 +2,164 @@
// Created by bastian on 31.08.22.
//
#include <cstdio>
#include <ignition/common/Console.hh>
#include <ignition/gazebo/Entity.hh>
#include <ignition/gazebo/Types.hh>
#include <ignition/gazebo/components/Component.hh>
#include <ignition/gazebo/components/Pose.hh>
#include <ignition/msgs/pose_trajectory.pb.h>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/server.hpp>
#include "ActorSystem.hpp"
#include <ignition/gazebo/components/Actor.hh>
#include <ignition/gazebo/components/Name.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <ignition/gazebo/Util.hh>
#include <sdf/Actor.hh>
IGNITION_ADD_PLUGIN(
ActorSystem,
ignition::gazebo::ActorSystem,
ignition::gazebo::System,
ActorSystem::ISystemPreUpdate,
ActorSystem::ISystemConfigure);
ignition::gazebo::ActorSystem::ISystemPreUpdate,
ignition::gazebo::ActorSystem::ISystemConfigure);
using namespace ignition;
using namespace gazebo;
ActorSystem::ActorSystem() = default;
ActorSystem::~ActorSystem() = default;
void ActorSystem::PreUpdate(const UpdateInfo &_info, ::EntityComponentManager &_ecm) {
switch(this->actorState){
case ANIMATION: {
//if (animationTarget == nullptr) {
// return;
//}
auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entitiy);
auto newTime = animationTimeComponent->Data() + _info.dt;
void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) {
auto test = std::chrono::duration_cast<std::chrono::milliseconds>(newTime);
ignwarn << "Animation state: " << test.count() << "\n";
*animationTimeComponent = components::AnimationTime(newTime);
_ecm.SetChanged(entitiy, components::AnimationTime::typeId, ComponentState::OneTimeChange);
return;
}
case MOVEMENT:{
if (movementTarget == nullptr) {
return;
}
auto trajPoseComp = _ecm.Component<components::TrajectoryPose>(this->entitiy);
auto actorPose = trajPoseComp->Data();
auto initialPose = actorPose;
auto targetPose = movementTarget.get()->target_position;
double distanceTraveled = (actorPose.Pos() - initialPose.Pos()).Length();
auto animTimeComp = _ecm.Component<components::AnimationTime>(
this->entitiy);
auto animTime = animTimeComp->Data() +
std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(
distanceTraveled *
1
)
);
*animTimeComp = components::AnimationTime(animTime);
return;
}
case IDLE:
return;
}
}
// Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc
// https://github.com/gazebosim/gz-sim/blob/ign-gazebo6/src/systems/follow_actor/FollowActor.cc
void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm, EventManager &) {
igndbg << "Plugin configuring..." << std::endl;
void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf,
ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &) {
printf("Plugin configuring...\n");
this->entitiy = _entity;
this->actorState = ActorSystemState::ANIMATION;
/*auto actorComp = _ecm.Component<Actor>(_entity);
if (!actorComp)
{
gzerr << "Entity [" << _entity << "] is not an actor." << std::endl;
auto actorComp = _ecm.Component<components::Actor>(_entity);
if (!actorComp) {
ignerr << "No attached actor found, terminating." << std::endl;
return;
}*/
}
igndbg << "Found this skin file: " << actorComp->Data().SkinFilename() << std::endl;
std::string animationName = "go";
if (!_sdf->HasElement("animation")) {
if (actorComp->Data().AnimationCount() < 1) {
ignerr << "Actor SDF doesn't have any animations." << std::endl;
return;
}
animationName = actorComp->Data().AnimationByIndex(0)->Name();
} else {
animationName = _sdf->Get<std::string>("animation");
}
igndbg << "Found this animation: " << animationName << std::endl;
animationName = "walk";
auto animationNameComp = _ecm.Component<components::AnimationName>(_entity);
if (nullptr == animationNameComp) {
_ecm.CreateComponent(_entity, components::AnimationName(animationName));
} else {
*animationNameComp = components::AnimationName(animationName);
}
for (const auto& elem: _ecm.ComponentTypes(_entity)) {
igndbg << "Component: " << elem << std::endl;
}
_ecm.SetChanged(_entity, components::AnimationName::typeId, ComponentState::OneTimeChange);
auto animTimeComp = _ecm.Component<components::AnimationTime>(_entity);
if (nullptr == animTimeComp) {
_ecm.CreateComponent(_entity, components::AnimationTime());
}
ignition::math::Pose3d initialPose;
auto poseComp = _ecm.Component<components::Pose>(_entity);
if (nullptr == poseComp) {
_ecm.CreateComponent(_entity, components::Pose(
ignition::math::Pose3d::Zero));
} else {
initialPose = poseComp->Data();
// We'll be setting the actor's X/Y pose with respect to the world. So we
// zero the current values.
auto newPose = initialPose;
newPose.Pos().X(0);
newPose.Pos().Y(0);
*poseComp = components::Pose(newPose);
}
auto trajPoseComp = _ecm.Component<components::TrajectoryPose>(_entity);
if (nullptr == trajPoseComp) {
// Leave Z to the pose component, control only 2D with Trajectory
initialPose.Pos().Z(0);
_ecm.CreateComponent(_entity, components::TrajectoryPose(initialPose));
}
rclcpp::init(0, {});
@ -53,7 +174,6 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
node = rclcpp::Node::make_shared("moveService", topic);
animationServer = rclcpp_action::create_server<ign_actor_plugin_msgs::action::Animation>(
node,
"animation",
@ -64,6 +184,7 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
},
[](AnimationServerGoalPtr PH1) {
return rclcpp_action::CancelResponse::ACCEPT;
return rclcpp_action::CancelResponse::REJECT;
},
[](AnimationServerGoalPtr PH1) {
@ -75,35 +196,19 @@ void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::
"movement",
[](rclcpp_action::GoalUUID goalUuid, MovementGoalPtr &movementGoal ){
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
},
[](MovementServerGoalPtr goalUuid ){
return rclcpp_action::CancelResponse::ACCEPT;
},
[](MovementServerGoalPtr goalUuid ){}
);
printf("Spinning node...\n");
igndbg << "Spinning node..." << std::endl;
std::thread spinThread([this]() {
while (true) {
printf("Wheee...\n");
igndbg << "Wheee..." << std::endl;
rclcpp::spin(node);
}
});
spinThread.detach();
}
/*
void loadAnimation(std::string name){
auto animationNameComp = _ecm.Component<components::AnimationName>(_entity);
if (nullptr == animationNameComp)
{
_ecm.CreateComponent(_entity, components::AnimationName(animationName));
}
else
{
*animationNameComp = components::AnimationName(animationName);
}
}
*/

27
src/ign_actor_plugin/src/ActorSystem.hpp
View File

@ -5,6 +5,7 @@
#ifndef BUILD_ACTORSYSTEM_H
#define BUILD_ACTORSYSTEM_H
#include <ignition/gazebo/Entity.hh>
#include <ignition/gazebo/System.hh>
#include <ignition/plugin/Register.hh>
#include <queue>
@ -20,15 +21,25 @@
#define MovementGoalPtr const std::shared_ptr<const ign_actor_plugin_msgs::action::Movement::Goal>
#define MovementServerGoalPtr const std::shared_ptr<rclcpp_action::ServerGoalHandle<ign_actor_plugin_msgs::action::Movement>>&
namespace ignition {
namespace gazebo {
enum ActorSystemState{
IDLE,MOVEMENT,ANIMATION
};
class ActorSystem:
public ignition::gazebo::System,
public ignition::gazebo::ISystemPreUpdate,
public ignition::gazebo::ISystemConfigure {
public System,
public ISystemPreUpdate,
public ISystemConfigure {
private:
std::shared_ptr<rclcpp::Node> node;
std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Animation>> animationServer;
std::shared_ptr<rclcpp_action::Server<ign_actor_plugin_msgs::action::Movement>> movementServer;
ActorSystemState actorState;
AnimationGoalPtr animationTarget;
MovementGoalPtr movementTarget;
Entity entitiy{kNullEntity};
public:
ActorSystem();
@ -37,16 +48,18 @@ public:
~ActorSystem() override;
public:
void PreUpdate(const ignition::gazebo::UpdateInfo &_info,
ignition::gazebo::EntityComponentManager &_ecm) override;
void PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm) override;
public:
void Configure(const ignition::gazebo::Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
ignition::gazebo::EntityComponentManager &_ecm,
ignition::gazebo::EventManager &/*_eventMgr*/) override;
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/) override;
};
}
}
#endif //BUILD_ACTORSYSTEM_H