From 6b650fe594d17a94d6102df402b5353f7a886f51 Mon Sep 17 00:00:00 2001 From: yenon Date: Wed, 12 Oct 2022 11:11:45 +0200 Subject: [PATCH] idk. --- src/gaz_simulation/world/gaz_ign.sdf | 4 +- src/ign_actor_plugin/CMakeLists.txt | 2 + src/ign_actor_plugin/src/ActorSystem.cpp | 55 +++++++++++++++++++----- src/ign_actor_plugin/src/ActorSystem.h | 31 ++++++------- 4 files changed, 60 insertions(+), 32 deletions(-) diff --git a/src/gaz_simulation/world/gaz_ign.sdf b/src/gaz_simulation/world/gaz_ign.sdf index 8b400e3..2e20d66 100644 --- a/src/gaz_simulation/world/gaz_ign.sdf +++ b/src/gaz_simulation/world/gaz_ign.sdf @@ -253,7 +253,7 @@ https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae - + 0 0 -9.8 6e-06 2.3e-05 -4.2e-05 diff --git a/src/ign_actor_plugin/CMakeLists.txt b/src/ign_actor_plugin/CMakeLists.txt index 494dfb9..6d8a08c 100644 --- a/src/ign_actor_plugin/CMakeLists.txt +++ b/src/ign_actor_plugin/CMakeLists.txt @@ -31,6 +31,8 @@ rosidl_target_interfaces(ActorPlugin target_link_libraries(ActorPlugin ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} ignition-gazebo5::ignition-gazebo5 + ${rclcpp_LIBRARIES} + ${rclcpp_action_LIBRARIES} ) ament_package() diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp index ba9b982..d14208c 100644 --- a/src/ign_actor_plugin/src/ActorSystem.cpp +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -5,41 +5,74 @@ #include #include "ActorSystem.h" +#define AnimationGoalPtr const std::shared_ptr +#define AnimationServerGoalPtr const std::shared_ptr>& + IGNITION_ADD_PLUGIN( ActorSystem, ignition::gazebo::System, - ActorSystem::ISystemPreUpdate) + ActorSystem::ISystemPreUpdate, + ActorSystem::ISystemConfigure) ActorSystem::ActorSystem() = default; ActorSystem::~ActorSystem() = default; void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) { - //_ecm.EachNew<>() } // Found too late: https://github.com/AlanSixth/gazebo_ros_action_tutorial/blob/master/src/gazebo_ros_action_tutorial.cc void ActorSystem::Configure(const ignition::gazebo::Entity &_entity, const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &) { + ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &) { + printf("Plugin configuring...\n"); + rclcpp::init(0, {}); + std::string topic = "ActorPlugin"; - if(_sdf->HasElement("topic")){ + if (_sdf->HasElement("topic")) { topic = _sdf->Get("topic"); } std::string name = "Actor"; - if(_sdf->HasElement("actor_name")){ + if (_sdf->HasElement("actor_name")) { name = _sdf->Get("actor_name"); } - node = rclcpp::Node::make_shared("moveService",topic); + node = rclcpp::Node::make_shared("moveService", topic); - rclcpp_action::create_server(node,"animation", - std::bind(&ActorSystem::handle_animation_goal,this,std::placeholders::_1,std::placeholders::_2), - std::bind(&ActorSystem::handle_animation_cancel,this,std::placeholders::_1), - std::bind(&ActorSystem::handle_animation_accepted,this,std::placeholders::_1) - ); +#pragma clang diagnostic push +#pragma ide diagnostic ignored "UnusedValue" + animationServer = rclcpp_action::create_server( + node, + "animation", + [](rclcpp_action::GoalUUID goalUuid, + AnimationGoalPtr &animationGoal) { + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + }, + [](AnimationServerGoalPtr PH1) { + return rclcpp_action::CancelResponse::ACCEPT; + }, + [](AnimationServerGoalPtr PH1) { + + } + ); + + movementServer rclcpp_action::create_server( + node, + "movement", + [](rclcpp_action::GoalUUID goalUuid, ) + ) +#pragma clang diagnostic pop + + printf("Spinning node...\n"); + std::thread spinThread([this]() { + while (true) { + printf("Wheee...\n"); + rclcpp::spin(node); + } + }); + spinThread.detach(); } /* diff --git a/src/ign_actor_plugin/src/ActorSystem.h b/src/ign_actor_plugin/src/ActorSystem.h index c37ee9d..69f2795 100644 --- a/src/ign_actor_plugin/src/ActorSystem.h +++ b/src/ign_actor_plugin/src/ActorSystem.h @@ -10,15 +10,17 @@ #include #include "rclcpp/rclcpp.hpp" #include "ign_actor_plugin/action/animation.hpp" - +#include "ign_actor_plugin/action/movement.hpp" class ActorSystem: public ignition::gazebo::System, public ignition::gazebo::ISystemPreUpdate, - public ignition::gazebo::ISystemConfigure{ + public ignition::gazebo::ISystemConfigure { private: std::shared_ptr node; + std::shared_ptr> animationServer; + std::shared_ptr> movementServer; public: ActorSystem(); @@ -26,26 +28,17 @@ public: public: ~ActorSystem() override; -public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, - ignition::gazebo::EntityComponentManager &_ecm) override; +public: + void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; -public: void Configure(const ignition::gazebo::Entity &_entity, - const std::shared_ptr &_sdf, - ignition::gazebo::EntityComponentManager &_ecm, - ignition::gazebo::EventManager &/*_eventMgr*/) override; +public: + void Configure(const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &/*_eventMgr*/) override; -private: rclcpp_action::GoalResponse handle_animation_goal(const rclcpp_action::GoalUUID & uuid, - std::shared_ptr goal); - -private: rclcpp_action::CancelResponse handle_animation_cancel( - const std::shared_ptr> goal_handle); - -private: void handle_animation_accepted( - const std::shared_ptr> goal_handle); }; - - - #endif //BUILD_ACTORSYSTEM_H