diff --git a/src/gazebo_ros_actor/CMakeLists.txt b/src/gazebo_ros_actor/CMakeLists.txt index c3e9793..9888e02 100644 --- a/src/gazebo_ros_actor/CMakeLists.txt +++ b/src/gazebo_ros_actor/CMakeLists.txt @@ -8,22 +8,22 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(gazebo REQUIRED) +#find_package(gazebo REQUIRED) -link_directories(${GAZEBO_LIBRARY_DIRS}) -include_directories(include ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ) +#link_directories(${GAZEBO_LIBRARY_DIRS}) +#include_directories(include ${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ) -include_directories( - include -) +#include_directories( +# include +#) -add_library(RosActorPlugin SHARED src/RosActorPlugin.cc) -target_link_libraries(RosActorPlugin ${GAZEBO_LIBRARIES}) -ament_target_dependencies(RosActorPlugin tf2_geometry_msgs rclcpp) +#add_library(RosActorPlugin SHARED src/RosActorPlugin.cc) +#target_link_libraries(RosActorPlugin ${GAZEBO_LIBRARIES}) +#ament_target_dependencies(RosActorPlugin tf2_geometry_msgs rclcpp) -install( - TARGETS - RosActorPlugin -) +#install( +# TARGETS +# RosActorPlugin +#) ament_package() diff --git a/src/ign_actor_plugin/CMakeLists.txt b/src/ign_actor_plugin/CMakeLists.txt index f20018e..494dfb9 100644 --- a/src/ign_actor_plugin/CMakeLists.txt +++ b/src/ign_actor_plugin/CMakeLists.txt @@ -1,16 +1,36 @@ cmake_minimum_required(VERSION 3.5.1) -project(ActorPlugin) +project(ign_actor_plugin) set(IGN_PLUGIN_VER 0) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(ignition-cmake2 REQUIRED) find_package(ignition-gazebo5 REQUIRED) + +find_package(rosidl_default_generators REQUIRED) + ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + +rosidl_generate_interfaces(${PROJECT_NAME} + "action/Animation.action" + "action/Movement.action" + ) + +ament_export_dependencies(rosidl_default_runtime) + # Add sources for each plugin to be registered. -add_library(SampleSystem src/ActorSystem.cpp) -set_property(TARGET SampleSystem PROPERTY CXX_STANDARD 17) -target_link_libraries(SampleSystem +add_library(ActorPlugin src/ActorSystem.cpp) +ament_target_dependencies(ActorPlugin rclcpp) +set_property(TARGET ActorPlugin PROPERTY CXX_STANDARD 17) + +rosidl_target_interfaces(ActorPlugin + ${PROJECT_NAME} "rosidl_typesupport_cpp") + +target_link_libraries(ActorPlugin ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} ignition-gazebo5::ignition-gazebo5 ) +ament_package() diff --git a/src/ign_actor_plugin/action/Animation.action b/src/ign_actor_plugin/action/Animation.action new file mode 100644 index 0000000..a3b9dc8 --- /dev/null +++ b/src/ign_actor_plugin/action/Animation.action @@ -0,0 +1,6 @@ +string animation_name +float32 animation_speed +--- +bool success +--- +float32 progress diff --git a/src/ign_actor_plugin/action/Movement.action b/src/ign_actor_plugin/action/Movement.action new file mode 100644 index 0000000..664f850 --- /dev/null +++ b/src/ign_actor_plugin/action/Movement.action @@ -0,0 +1,8 @@ +string animation_name +float32 animation_speed +float32[3] target_position +float32[3] target_orientation +--- +bool success +--- +float32 progress diff --git a/src/ign_actor_plugin/package.xml b/src/ign_actor_plugin/package.xml new file mode 100644 index 0000000..31a2061 --- /dev/null +++ b/src/ign_actor_plugin/package.xml @@ -0,0 +1,28 @@ + + + + ign_actor_plugin + 0.0.0 + Plugin for Gazebo Ignition to remote control actors + Bastian Hofmann + TODO: License declaration + + ament_cmake + rosidl_default_generators + rclcpp + rclcpp_action + rclcpp_components + ign_actor_plugin_messages + action_msgs + ignition-cmake2 + ignition-gazebo5 + rosidl_default_runtime + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp index 23f62fe..ba9b982 100644 --- a/src/ign_actor_plugin/src/ActorSystem.cpp +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -2,6 +2,7 @@ // Created by bastian on 31.08.22. // +#include #include "ActorSystem.h" IGNITION_ADD_PLUGIN( @@ -15,4 +16,42 @@ 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 &) { + rclcpp::init(0, {}); + std::string topic = "ActorPlugin"; + if(_sdf->HasElement("topic")){ + topic = _sdf->Get("topic"); + } + std::string name = "Actor"; + if(_sdf->HasElement("actor_name")){ + name = _sdf->Get("actor_name"); + } + + 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) + ); +} + +/* +void loadAnimation(std::string name){ + auto animationNameComp = _ecm.Component(_entity); + if (nullptr == animationNameComp) + { + _ecm.CreateComponent(_entity, components::AnimationName(animationName)); + } + else + { + *animationNameComp = components::AnimationName(animationName); + } +} +*/ \ No newline at end of file diff --git a/src/ign_actor_plugin/src/ActorSystem.h b/src/ign_actor_plugin/src/ActorSystem.h index 04f6012..c37ee9d 100644 --- a/src/ign_actor_plugin/src/ActorSystem.h +++ b/src/ign_actor_plugin/src/ActorSystem.h @@ -7,10 +7,18 @@ #include #include +#include +#include "rclcpp/rclcpp.hpp" +#include "ign_actor_plugin/action/animation.hpp" + class ActorSystem: public ignition::gazebo::System, - public ignition::gazebo::ISystemPreUpdate { + public ignition::gazebo::ISystemPreUpdate, + public ignition::gazebo::ISystemConfigure{ + +private: + std::shared_ptr node; public: ActorSystem(); @@ -20,7 +28,24 @@ public: 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; + +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