From 17e61a505f1cc8c7cbb26fa17d821feb9bbf8a51 Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Tue, 14 Feb 2023 17:03:17 +0000 Subject: [PATCH] Final version of actor plugin --- src/cpp_pubsub/CMakeLists.txt | 38 -- .../src/publisher_member_function.cpp | 52 -- .../src/subscriber_member_function.cpp | 66 -- src/gz_ros2_control | 1 - src/ign_actor_plugin/CMakeLists.txt | 41 +- src/ign_actor_plugin/package.xml | 3 +- src/ign_actor_plugin/src/ActorSystem.cpp | 564 +++++++----------- src/ign_actor_plugin/src/ActorSystem.hpp | 35 +- src/ros_actor_action_server/CMakeLists.txt | 57 ++ .../package.xml | 5 +- src/ros_actor_action_server/src/Server.cpp | 169 ++++++ src/ros_actor_action_server/src/Server.hpp | 29 + .../CMakeLists.txt | 3 +- .../action/Animation.action | 0 .../action/Movement.action | 0 .../package.xml | 4 +- .../CMakeLists.txt | 28 + .../MessageTypes.hpp | 17 + src/ros_actor_message_queue_msgs/package.xml | 15 + 19 files changed, 579 insertions(+), 548 deletions(-) delete mode 100644 src/cpp_pubsub/CMakeLists.txt delete mode 100644 src/cpp_pubsub/src/publisher_member_function.cpp delete mode 100644 src/cpp_pubsub/src/subscriber_member_function.cpp delete mode 160000 src/gz_ros2_control create mode 100644 src/ros_actor_action_server/CMakeLists.txt rename src/{cpp_pubsub => ros_actor_action_server}/package.xml (82%) create mode 100644 src/ros_actor_action_server/src/Server.cpp create mode 100644 src/ros_actor_action_server/src/Server.hpp rename src/{ign_actor_plugin_msgs => ros_actor_action_server_msgs}/CMakeLists.txt (73%) rename src/{ign_actor_plugin_msgs => ros_actor_action_server_msgs}/action/Animation.action (100%) rename src/{ign_actor_plugin_msgs => ros_actor_action_server_msgs}/action/Movement.action (100%) rename src/{ign_actor_plugin_msgs => ros_actor_action_server_msgs}/package.xml (81%) create mode 100644 src/ros_actor_message_queue_msgs/CMakeLists.txt create mode 100644 src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp create mode 100644 src/ros_actor_message_queue_msgs/package.xml diff --git a/src/cpp_pubsub/CMakeLists.txt b/src/cpp_pubsub/CMakeLists.txt deleted file mode 100644 index 3a77c56..0000000 --- a/src/cpp_pubsub/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION VERSION 3.5.1) -project(cpp_pubsub) - -set(CMAKE_CXX_STANDARD 17) -add_compile_options(-Wall -Wextra -Wpedantic) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -# find dependencies - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -add_executable(talker src/publisher_member_function.cpp) -ament_target_dependencies(talker geometry_msgs rclcpp) -add_executable(listener src/subscriber_member_function.cpp) -ament_target_dependencies(listener geometry_msgs rclcpp) - -install(TARGETS - talker - listener - DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/cpp_pubsub/src/publisher_member_function.cpp b/src/cpp_pubsub/src/publisher_member_function.cpp deleted file mode 100644 index 2b46cb8..0000000 --- a/src/cpp_pubsub/src/publisher_member_function.cpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. -// -// 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. - -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; - -/* This example creates a subclass of Node and uses std::bind() to register a - * member function as a callback from the timer. */ - -class MinimalPublisher : public rclcpp::Node{ - public: - MinimalPublisher(): Node("minimal_publisher") { - publisher_ = this->create_publisher("targetPose", 10); - timer_ = this->create_wall_timer( - 500ms, [this]{ - auto message = geometry_msgs::msg::Pose(); - message.position.x=i; - RCLCPP_INFO(this->get_logger(), "Publishing: '%f %f %f'", message.position.x,message.position.y,message.position.z); - i++; - publisher_->publish(message); - }); - } - - private: - int i=0; - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; -}; - -int main(int argc, char *argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/cpp_pubsub/src/subscriber_member_function.cpp b/src/cpp_pubsub/src/subscriber_member_function.cpp deleted file mode 100644 index 6a0b945..0000000 --- a/src/cpp_pubsub/src/subscriber_member_function.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. -// -// 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. - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using std::placeholders::_1; -/* -class MinimalSubscriber : public rclcpp::Node -{ -public: - MinimalSubscriber(): Node("minimal_subscriber"){} - -private: - rclcpp::Subscription::SharedPtr subscription_ = this->create_subscription( - "topic", 10, [this](const geometry_msgs::msg::Pose PH1) { - RCLCPP_INFO(this->get_logger(), "I heard: '%f %f %f'", PH1.position.x,PH1.position.y,PH1.position.z); - - //RCUTILS_LOG_INFO_NAMED(this->get_logger().get_name(),"I heard: '%s'", PH1->data.c_str()); - }); -}; -*/ -template -class MinimalSubscriber : public rclcpp::Node -{ -public: MinimalSubscriber(const std::string& node_name, const std::string& topic_name, void(*fun)(T)): Node(node_name){ - this->subscription_ = this->create_subscription( - topic_name, 10, [fun](const T result) { - fun(result); - }); - } - -private: - typename rclcpp::Subscription::SharedPtr subscription_; -}; - -template -std::thread spinMinimalSubscriber(const std::string& node_name, const std::string& topic_name, void(*callback)(T)){ - return std::thread([node_name,topic_name,callback](){ - rclcpp::spin(std::make_shared>(node_name,topic_name,callback)); - rclcpp::shutdown(); - }); -} - -int main(int argc, char * argv[]) -{ - rclcpp::init(0, nullptr); - spinMinimalSubscriber("talker","topic",[](geometry_msgs::msg::Pose pose){ - printf("log: %f %f %f \n",pose.position.x,pose.position.y,pose.position.z); - }).join(); - return 0; -} \ No newline at end of file diff --git a/src/gz_ros2_control b/src/gz_ros2_control deleted file mode 160000 index b0c29c2..0000000 --- a/src/gz_ros2_control +++ /dev/null @@ -1 +0,0 @@ -Subproject commit b0c29c2f3245eca25753fbe2527eeacc7cb9dac0 diff --git a/src/ign_actor_plugin/CMakeLists.txt b/src/ign_actor_plugin/CMakeLists.txt index e7f9c41..7281ee8 100644 --- a/src/ign_actor_plugin/CMakeLists.txt +++ b/src/ign_actor_plugin/CMakeLists.txt @@ -10,33 +10,36 @@ find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) find_package(ignition-cmake2 REQUIRED) find_package(ignition-gazebo6 REQUIRED) -find_package(ign_actor_plugin_msgs REQUIRED) +find_package(ros_actor_message_queue_msgs REQUIRED) +find_package(ros_actor_action_server_msgs REQUIRED) find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -ament_export_dependencies(ActorPlugin - "rosidl_default_runtime" - "ign_actor_plugin_msgs" - "rclcpp" - "rclcpp_action" - "rclcpp_components") +ament_export_dependencies(ign_actor_plugin + "rosidl_default_runtime" + "ros_actor_action_server_msgs" + "rclcpp" + "rclcpp_action" + "rclcpp_components" +) # Add sources for each plugin to be registered. -add_library(ActorPlugin SHARED src/ActorSystem.cpp) -ament_target_dependencies(ActorPlugin rclcpp rclcpp_action ign_actor_plugin_msgs) -set_property(TARGET ActorPlugin PROPERTY CXX_STANDARD 17) -target_compile_options(ActorPlugin PRIVATE -std=c++17) +add_library(ign_actor_plugin SHARED src/ActorSystem.cpp) +ament_target_dependencies(ign_actor_plugin rclcpp rclcpp_action ros_actor_message_queue_msgs ros_actor_action_server_msgs) +set_property(TARGET ign_actor_plugin PROPERTY CXX_STANDARD 17) +target_compile_options(ign_actor_plugin PRIVATE -std=c++17) -target_link_libraries(ActorPlugin - ignition-gazebo6::ignition-gazebo6 - ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} - ${rclcpp_LIBRARIES} - ${rclcpp_action_LIBRARIES} - ) +target_link_libraries(ign_actor_plugin + ignition-gazebo6::ignition-gazebo6 + ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + ${rclcpp_LIBRARIES} + ${rclcpp_action_LIBRARIES} +) install(TARGETS - ActorPlugin - DESTINATION lib/${PROJECT_NAME}) + ign_actor_plugin + DESTINATION lib/${PROJECT_NAME} +) ament_package() diff --git a/src/ign_actor_plugin/package.xml b/src/ign_actor_plugin/package.xml index 24b77b1..e9c0dfb 100644 --- a/src/ign_actor_plugin/package.xml +++ b/src/ign_actor_plugin/package.xml @@ -11,7 +11,8 @@ rclcpp rclcpp_action rclcpp_components - ign_actor_plugin_msgs + ros_actor_message_queue_msgs + ros_actor_action_server_msgs ignition-cmake2 ignition-gazebo6 ament_lint_auto diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp index b47b623..e6093dc 100644 --- a/src/ign_actor_plugin/src/ActorSystem.cpp +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -2,42 +2,37 @@ // Created by bastian on 31.08.22. // +#include "ActorSystem.hpp" #include -#include +#include +#include #include +#include +#include +#include #include +#include #include +#include #include #include #include #include -#include "ActorSystem.hpp" -#include -#include #include -#include -#include -#include #include -#include -#include #include -IGNITION_ADD_PLUGIN( - ignition::gazebo::ActorSystem, - ignition::gazebo::System, - ignition::gazebo::ActorSystem::ISystemPreUpdate, - ignition::gazebo::ActorSystem::ISystemConfigure) - +IGNITION_ADD_PLUGIN(ignition::gazebo::ActorSystem, ignition::gazebo::System, ignition::gazebo::ActorSystem::ISystemPreUpdate, + ignition::gazebo::ActorSystem::ISystemConfigure) using namespace ignition::gazebo; -using namespace ign_actor_plugin_msgs; +using namespace ros_actor_action_server_msgs; ActorSystem::ActorSystem() = default; ActorSystem::~ActorSystem() = default; -void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char* animationName){ +void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char *animationName) { igndbg << "Initial setup for new animation." << std::endl; auto actorComp = _ecm.Component(entity); @@ -51,15 +46,15 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char* animationN _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); int foundAnimationIndex = -1; - for (int animationIndex = 0 ; animationIndex < actorComp->Data().AnimationCount() ; animationIndex++) { + for (int animationIndex = 0; animationIndex < actorComp->Data().AnimationCount(); animationIndex++) { auto animation = actorComp->Data().AnimationByIndex(animationIndex); - if (animation->Name() == animationName){ + if (animation->Name() == animationName) { foundAnimationIndex = animationIndex; break; } } - if(foundAnimationIndex != -1){ + if (foundAnimationIndex != -1) { igndbg << "Found animation to play" << std::endl; auto fileName = actorComp->Data().AnimationByIndex(foundAnimationIndex)->Filename(); @@ -77,317 +72,230 @@ void ActorSystem::switchAnimation(EntityComponentManager &_ecm, char* animationN igndbg << "Translation: " << currentTranslationAlign << std::endl; igndbg << "Rotation: " << currentRotationAlign << std::endl; - if (currentSkeleton != nullptr){ + if (currentSkeleton != nullptr) { duration = currentSkeleton->Animation(0)->Length(); igndbg << "length: " << duration << " seconds." << std::endl; } else { ignerr << "No skeleton found!" << std::endl; } - auto initialPose = currentRootNodeAnimation -> FrameAt(0).Pose(); + auto initialPose = currentRootNodeAnimation->FrameAt(0).Pose(); auto poseComponent = _ecm.Component(entity); auto newPose = initialPose; newPose.Pos().X(0); newPose.Pos().Y(0); *poseComponent = components::Pose(newPose); - } else { - ignerr << "No animation found!" << std::endl; + ignerr << "No animation found!" << std::endl; } } void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) { - pthread_mutex_lock(&shared_mmap->mutex); - switch(shared_mmap->currentState){ - case SETUP: { - igndbg << "Setting up actor plugin" << std::endl; + threadMutex.lock(); - auto actorComponent = _ecm.Component(entity); + - auto animationNameComponent = _ecm.Component(entity); - if (nullptr == animationNameComponent) { - _ecm.CreateComponent(entity, components::AnimationName(actorComponent->Data().AnimationByIndex(0)->Name())); - } + switch (currentState) { + case SETUP: { + igndbg << "Setting up actor plugin" << std::endl; - auto animTimeComponent = _ecm.Component(entity); - if (nullptr == animTimeComponent) { - _ecm.CreateComponent(entity, components::AnimationTime()); - } + auto actorComponent = _ecm.Component(entity); - auto poseComponent = _ecm.Component(entity); - if (nullptr == poseComponent) { - _ecm.CreateComponent(entity, components::Pose(ignition::math::Pose3d::Zero)); - } - - auto trajectoryPoseComponent = _ecm.Component(entity); - if (nullptr == trajectoryPoseComponent) { - _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero)); - } - - shared_mmap->currentState = ActorSystemState::IDLE; - break; + auto animationNameComponent = _ecm.Component(entity); + if (nullptr == animationNameComponent) { + _ecm.CreateComponent(entity, components::AnimationName(actorComponent->Data().AnimationByIndex(0)->Name())); } - case ANIMATION: { - if (shared_mmap->lastState == ActorSystemState::IDLE){ - switchAnimation(_ecm, shared_mmap->animation_name); - } - auto animationTimeComponent = _ecm.Component(this->entity); - auto newTime = animationTimeComponent->Data() + _info.dt; - auto newTimeSeconds = std::chrono::duration(newTime).count(); - auto feedback = (float) (newTimeSeconds/duration); - write(feedback_pipe[1],&feedback,sizeof(float)); - //currentAnimationGoalPtr->publish_feedback(feedback); - - if (newTimeSeconds >= duration){ - //currentAnimationGoalPtr->succeed(std::make_shared()); - igndbg << "Animation " << shared_mmap->animation_name << " finished." << std::endl; - shared_mmap->currentState = ActorSystemState::IDLE; - break; - } - - *animationTimeComponent = components::AnimationTime(newTime); - _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); - - - /* - if(currentRootNodeAnimation == nullptr){ - break; - } - - auto rootTransformation = currentRootNodeAnimation->FrameAt(newTimeSeconds,false); - math::Matrix4d totalTf = currentTranslationAlign * rootTransformation * currentRotationAlign; - - auto translation = totalTf.Translation(); - translation[0] = 0; - totalTf.SetTranslation(translation); - - auto poseComp = _ecm.Component(entity); - *poseComp = components::Pose (totalTf.Pose()); - _ecm.SetChanged(entity, components::Pose::typeId, ComponentState::OneTimeChange); - */ - - break; + auto animTimeComponent = _ecm.Component(entity); + if (nullptr == animTimeComponent) { + _ecm.CreateComponent(entity, components::AnimationTime()); } - case MOVEMENT:{ - if (shared_mmap->lastState == ActorSystemState::IDLE){ - igndbg << "Starting Movement..." << std::endl; - switchAnimation(_ecm, shared_mmap->animation_name); - } - auto trajectoryPoseComp = _ecm.Component(this->entity); - auto actorPose = trajectoryPoseComp->Data(); - auto targetPose = shared_mmap->target_position; - - auto targetDirection = math::Vector3d(targetPose[0],targetPose[1],targetPose[2]) - actorPose.Pos(); - - if (targetDirection.Length()<0.05){ - float feedback = 1.0f; - write(feedback_pipe[1],&feedback,sizeof(float)); - //currentMovementGoalPtr->succeed(std::make_shared()); - shared_mmap->currentState = ActorSystemState::IDLE; - break; - } - - auto animationTimeComponent = _ecm.Component(this->entity); - auto oldTimeSeconds = std::chrono::duration(animationTimeComponent->Data()).count(); - auto deltaTimeSeconds = std::chrono::duration(_info.dt).count(); - auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds; - auto newTime = animationTimeComponent->Data() + _info.dt; - - if(newTimeSeconds >= duration){ - newTimeSeconds -= duration; - newTime = std::chrono::duration_cast(std::chrono::duration(newTimeSeconds)); - } - - auto targetYaw = atan2(targetDirection.Y(),targetDirection.X()); - auto currentYaw = fmod(actorPose.Rot().Yaw(),M_PI); - - auto angularDirection = fmod((targetYaw - currentYaw), M_PI+0.001); // additional 0.001 rad to prevent instant flip through 180 rotation - auto turnSpeed = 1.0; - - if (angularDirection < 0){ - turnSpeed = -turnSpeed; - } - - if (abs(angularDirection) > 0.01){ - actorPose.Rot().Euler(0,0,actorPose.Rot().Yaw()+(turnSpeed*deltaTimeSeconds)); - *trajectoryPoseComp = components::TrajectoryPose(actorPose); - _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange); - }else{ - if (nullptr == currentRootNodeAnimation){ - ignerr << "Current animation doesn't move root node, this is unsupported for movement animations." << std::endl; - break; - } - - double distance = (deltaTimeSeconds/duration)*shared_mmap->animation_distance; - - actorPose.Pos() += targetDirection.Normalize() * distance; - - actorPose.Rot().Euler(0,0,targetYaw); - *trajectoryPoseComp = components::TrajectoryPose(actorPose); - *animationTimeComponent = components::AnimationTime(newTime); - - _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange); - _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); - } - break; + auto poseComponent = _ecm.Component(entity); + if (nullptr == poseComponent) { + _ecm.CreateComponent(entity, components::Pose(ignition::math::Pose3d::Zero)); } - case IDLE: - if (shared_mmap->lastState != shared_mmap->currentState){ - igndbg << "Now idling..." << std::endl; - } - break; + + auto trajectoryPoseComponent = _ecm.Component(entity); + if (nullptr == trajectoryPoseComponent) { + _ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero)); + } + + currentState = IDLE; + break; } - shared_mmap->lastState = shared_mmap->currentState; - pthread_mutex_unlock(&shared_mmap->mutex); + case ANIMATION: { + if (lastState == IDLE) { + switchAnimation(_ecm, animation_name); + } + auto animationTimeComponent = _ecm.Component(this->entity); + auto newTime = animationTimeComponent->Data() + _info.dt; + auto newTimeSeconds = std::chrono::duration(newTime).count(); + + auto feedback = newTimeSeconds / duration; + sendFeedback(feedback); + + if (newTimeSeconds >= duration) { + igndbg << "Animation " << animation_name << " finished." << std::endl; + currentState = IDLE; + break; + } + + *animationTimeComponent = components::AnimationTime(newTime); + _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); + + /* + if(currentRootNodeAnimation == nullptr){ + break; + } + + auto rootTransformation = currentRootNodeAnimation->FrameAt(newTimeSeconds,false); + math::Matrix4d totalTf = currentTranslationAlign * rootTransformation * currentRotationAlign; + + auto translation = totalTf.Translation(); + translation[0] = 0; + totalTf.SetTranslation(translation); + + auto poseComp = _ecm.Component(entity); + *poseComp = components::Pose (totalTf.Pose()); + _ecm.SetChanged(entity, components::Pose::typeId, ComponentState::OneTimeChange); + */ + + break; + } + case MOVEMENT: { + if (lastState == IDLE) { + igndbg << "Starting Movement..." << std::endl; + switchAnimation(_ecm, animation_name); + } + + auto trajectoryPoseComp = _ecm.Component(this->entity); + auto actorPose = trajectoryPoseComp->Data(); + auto targetPose = target_position; + + auto targetDirection = math::Vector3d(targetPose[0], targetPose[1], targetPose[2]) - actorPose.Pos(); + + if (targetDirection.Length() < 0.05) { + sendFeedback(1.0); + currentState = IDLE; + break; + } + + auto animationTimeComponent = _ecm.Component(this->entity); + auto oldTimeSeconds = std::chrono::duration(animationTimeComponent->Data()).count(); + auto deltaTimeSeconds = std::chrono::duration(_info.dt).count(); + auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds; + auto newTime = animationTimeComponent->Data() + _info.dt; + + if (newTimeSeconds >= duration) { + newTimeSeconds -= duration; + newTime = std::chrono::duration_cast(std::chrono::duration(newTimeSeconds)); + } + + auto targetYaw = atan2(targetDirection.Y(), targetDirection.X()); + auto currentYaw = fmod(actorPose.Rot().Yaw(), M_PI); + + auto angularDirection = fmod((targetYaw - currentYaw), + M_PI + 0.001); // additional 0.001 rad to prevent instant flip through 180 rotation + auto turnSpeed = 1.0; + + if (angularDirection < 0) { + turnSpeed = -turnSpeed; + } + + if (abs(angularDirection) > 0.01) { + actorPose.Rot().Euler(0, 0, actorPose.Rot().Yaw() + (turnSpeed * deltaTimeSeconds)); + *trajectoryPoseComp = components::TrajectoryPose(actorPose); + _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange); + } else { + if (nullptr == currentRootNodeAnimation) { + ignerr << "Current animation doesn't move root node, this is unsupported for movement animations." << std::endl; + break; + } + + double distance = (deltaTimeSeconds / duration) * animation_distance; + + actorPose.Pos() += targetDirection.Normalize() * distance; + + actorPose.Rot().Euler(0, 0, targetYaw); + *trajectoryPoseComp = components::TrajectoryPose(actorPose); + *animationTimeComponent = components::AnimationTime(newTime); + + _ecm.SetChanged(entity, components::TrajectoryPose::typeId, ComponentState::OneTimeChange); + _ecm.SetChanged(entity, components::AnimationTime::typeId, ComponentState::OneTimeChange); + } + break; + } + case IDLE: + break; + } + + if (lastState != currentState) { + igndbg << "State change: " << lastState << " -> " << currentState << std::endl; + sendFeedback(0.0); + } + + lastState = currentState; + threadMutex.unlock(); } +void ActorSystem::messageQueueInterface(const char name[256]) { + mq_attr queueAttributes; + queueAttributes.mq_flags = 0; + queueAttributes.mq_curmsgs = 0; + queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::FeedbackMessage); + queueAttributes.mq_maxmsg = 1; + char generatedName[256]; + strcpy(generatedName,name); + strcat(generatedName,"Feedback"); + feedbackQueue = mq_open(generatedName, O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes); -void ActorSystem::rclcppServer(const std::string& topic){ - rclcpp::init(0, {}); - - auto node = rclcpp::Node::make_shared("moveService", topic); - - AnimationServerGoalPtr currentAnimationGoalPtr; - MovementServerGoalPtr currentMovementGoalPtr; - -#pragma clang diagnostic push -#pragma ide diagnostic ignored "UnusedValue" - auto animationServer = rclcpp_action::create_server( - node, - "animation", - [this](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) { - igndbg << "goal" << shared_mmap->currentState << std::endl; - if (shared_mmap->currentState == ActorSystemState::IDLE) { - pthread_mutex_lock(&shared_mmap->mutex); - igndbg << "goal l" << shared_mmap->currentState << std::endl; - - strcpy(shared_mmap->animation_name,animationGoal->animation_name.data()); - - pthread_mutex_unlock(&shared_mmap->mutex); - return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; - }else{ - return rclcpp_action::GoalResponse::REJECT; - } - }, - [this](const AnimationServerGoalPtr& animationGoalPtr) { - igndbg << "cancel" << shared_mmap->currentState << std::endl; - if (shared_mmap->currentState == ActorSystemState::IDLE) { - return rclcpp_action::CancelResponse::REJECT; - }else{ - pthread_mutex_lock(&shared_mmap->mutex); - - shared_mmap->currentState = ActorSystemState::IDLE; - pthread_mutex_unlock(&shared_mmap->mutex); - - return rclcpp_action::CancelResponse::ACCEPT; - } - }, - [this,¤tAnimationGoalPtr](const AnimationServerGoalPtr& animationGoalPtr) { - igndbg << "accepted" << shared_mmap->currentState << std::endl; - pthread_mutex_lock(&shared_mmap->mutex); - igndbg << "accepted l" << shared_mmap->currentState << std::endl; - - igndbg << "got animation handle" << std::endl; - currentAnimationGoalPtr = animationGoalPtr; - - shared_mmap->currentState = ActorSystemState::ANIMATION; - currentAnimationGoalPtr->execute(); - - pthread_mutex_unlock(&shared_mmap->mutex); - - } - ); - - auto movementServer = rclcpp_action::create_server( - node, - "movement", - [this](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal ){ - if (shared_mmap->currentState == ActorSystemState::IDLE) { - pthread_mutex_lock(&shared_mmap->mutex); - - shared_mmap->target_position = math::Vector3d( - movementGoal->target_position[0], - movementGoal->target_position[1], - movementGoal->target_position[2]); - shared_mmap->animation_distance = movementGoal->animation_distance; - strcpy(shared_mmap->animation_name,movementGoal->animation_name.data()); - - pthread_mutex_unlock(&shared_mmap->mutex); - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; - }else{ - return rclcpp_action::GoalResponse::REJECT; - } - }, - [this](const MovementServerGoalPtr& movementGoalPtr ){ - if (shared_mmap->currentState == ActorSystemState::IDLE) { - return rclcpp_action::CancelResponse::REJECT; - }else{ - pthread_mutex_lock(&shared_mmap->mutex); - - shared_mmap->currentState = ActorSystemState::IDLE; - - pthread_mutex_unlock(&shared_mmap->mutex); - return rclcpp_action::CancelResponse::ACCEPT; - } - }, - [this,¤tMovementGoalPtr](const MovementServerGoalPtr& movementGoalPtr ){ - pthread_mutex_lock(&shared_mmap->mutex); - - igndbg << "got movement handle" << std::endl; - currentMovementGoalPtr = movementGoalPtr; - shared_mmap->currentState = ActorSystemState::MOVEMENT; - - pthread_mutex_unlock(&shared_mmap->mutex); - } - ); -#pragma clang diagnostic pop - - std::thread t([this,¤tAnimationGoalPtr,¤tMovementGoalPtr]() { - float progress; - while(true) { - read(feedback_pipe[0], &progress, sizeof(float)); - - igndbg << progress << std::endl; - - if(progress >= 1.0f){ - if(currentAnimationGoalPtr!=nullptr){ - - currentAnimationGoalPtr->succeed(std::make_shared()); - } - if(currentMovementGoalPtr!=nullptr){ - currentMovementGoalPtr->succeed(std::make_shared()); - } - }else{ - if(currentAnimationGoalPtr!=nullptr){ - auto feedback = std::make_shared(); - feedback->set__progress(progress); - currentAnimationGoalPtr->publish_feedback(feedback); - } - if(currentMovementGoalPtr!=nullptr){ - auto feedback = std::make_shared(); - feedback->set__progress(progress); - currentMovementGoalPtr->publish_feedback(feedback); - } - } - usleep(500); - } - }); - t.detach(); - - igndbg << "Spinning node..." << std::endl; - while (true) { - printf("Spinning...\n"); - rclcpp::spin(node); + if (feedbackQueue == (mqd_t)-1) { + ignerr << "Could not create queue. (" << errno << ")" << std::endl; + return; } + + strcpy(generatedName,name); + strcat(generatedName,"Action"); + queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::ActionMessage); + actionQueue = mq_open(generatedName, O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes); + + if (actionQueue == (mqd_t)-1) { + ignerr << "Could not create queue. (" << errno << ")" << std::endl; + return; + } + + std::thread([this] { + ActionMessage receivedAction; + while (true) { + mq_receive(actionQueue, (char *)&receivedAction, sizeof(ActionMessage), nullptr); + threadMutex.lock(); + + strcpy(animation_name, receivedAction.animationName); + // animation_speed = receivedAction.animationSpeed; + animation_distance = receivedAction.animationDistance; + target_position = math::Vector3d(receivedAction.positionX, receivedAction.positionY, receivedAction.positionZ); + currentState = receivedAction.state; + + std::cout << "Got Action" << std::endl; + threadMutex.unlock(); + } + }).detach(); +} + +void ActorSystem::sendFeedback(double progress){ + FeedbackMessage message; + message.progress = progress; + message.state = currentState; + mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0); } // 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 &_sdf, - EntityComponentManager &_ecm, EventManager &) { +void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, + EventManager &) { + igndbg << "actor plugin configuring..." << std::endl; auto actorComp = _ecm.Component(_entity); @@ -396,62 +304,20 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptrHasElement("topic")) { - topic = _sdf->Get("topic"); + auto topicString = _sdf->Get("topic"); + if (topicString.size() >= 256) { + ignerr << "queue name too long, not starting plugin!"; + return; + } + strcpy(topic, topicString.c_str()); } - + this->entity = _entity; - igndbg << "shmem setup" << std::endl; - int fd = open(".", O_TMPFILE|O_RDWR, 00600); - auto structSize = sizeof(struct sharedMmap); - if(ftruncate(fd, structSize)!=0){ - ignerr << "ftruncate" << std::endl; - return; - } - shared_mmap = (struct sharedMmap *) mmap(nullptr,structSize,PROT_READ|PROT_WRITE,MAP_SHARED,fd,0); - //close(fd); + messageQueueInterface(topic); - if (shared_mmap == MAP_FAILED){ - ignerr << "MAP_FAILED" << std::endl; - return; - } - - pthread_mutexattr_t mutexattr; - if(pthread_mutexattr_init(&mutexattr)!=0){ - ignerr << "pthread_mutexattr_init" << std::endl; - return; - } - if(pthread_mutexattr_setpshared(&mutexattr, PTHREAD_PROCESS_SHARED)!=0){ - ignerr << "pthread_mutexattr_setpshared" << std::endl; - return; - } - if(pthread_mutex_init(&shared_mmap->mutex, &mutexattr)!=0){ - ignerr << "pthread_mutex_init" << std::endl; - return; - } - - shared_mmap->currentState = ActorSystemState::SETUP; - shared_mmap->lastState = ActorSystemState::IDLE; - - igndbg << "pipe setup" << std::endl; - - if (pipe(feedback_pipe)!=0){ - igndbg << "pipe failed." << std::endl; - return; - } - - switch(fork()){ - case -1: - ignerr << "Could not fork." << std::endl; - break; - case 0: - ignmsg << "Child fork." << std::endl; - rclcppServer(topic); - break; - default: - ignmsg << "Parent fork." << std::endl; - break; - } + currentState = SETUP; + lastState = SETUP; } \ No newline at end of file diff --git a/src/ign_actor_plugin/src/ActorSystem.hpp b/src/ign_actor_plugin/src/ActorSystem.hpp index 5c34418..d282fad 100644 --- a/src/ign_actor_plugin/src/ActorSystem.hpp +++ b/src/ign_actor_plugin/src/ActorSystem.hpp @@ -14,14 +14,19 @@ #include #include #include -#include -#include +#include +#include +#include #include #include #include #include +#include +#include +#include -using namespace ign_actor_plugin_msgs; +using namespace ros_actor_message_queue_msgs; +using namespace ros_actor_action_server_msgs; using rclcpp_action::ServerGoalHandle; #define AnimationActionServer rclcpp_action::Server @@ -35,17 +40,6 @@ using rclcpp_action::ServerGoalHandle; namespace ignition { namespace gazebo { - enum ActorSystemState { - SETUP, IDLE, MOVEMENT, ANIMATION - }; - - struct sharedMmap{ - pthread_mutex_t mutex; - ActorSystemState currentState,lastState; - char animation_name[256]; - double animation_distance; - math::Vector3d target_position; - }; class ActorSystem : public System, @@ -61,14 +55,18 @@ namespace ignition { //action::Movement::Goal movementTarget; //AnimationServerGoalPtr currentAnimationGoalPtr; //MovementServerGoalPtr currentMovementGoalPtr; + math::Vector3d target_position; + double animation_distance; + char animation_name[256]; + ActorPluginState currentState,lastState; double duration{}; Entity entity{kNullEntity}; - //std::mutex threadMutex; + std::mutex threadMutex; std::shared_ptr currentSkeleton; common::NodeAnimation* currentRootNodeAnimation; math::Matrix4 currentRotationAlign, currentTranslationAlign; - struct sharedMmap* shared_mmap; - int feedback_pipe[2]; + mqd_t feedbackQueue; + mqd_t actionQueue; public: ActorSystem(); @@ -88,7 +86,8 @@ namespace ignition { private: void switchAnimation(EntityComponentManager &_ecm, char *animationName); - void rclcppServer(const std::string& topic); + void messageQueueInterface(const char name[256]); + void sendFeedback(double feedback); }; diff --git a/src/ros_actor_action_server/CMakeLists.txt b/src/ros_actor_action_server/CMakeLists.txt new file mode 100644 index 0000000..ba4b880 --- /dev/null +++ b/src/ros_actor_action_server/CMakeLists.txt @@ -0,0 +1,57 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) +project(ros_actor_action_server) + +add_compile_options(-Wall -Wextra -Wpedantic) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + + +# find dependencies + +set(DEPENDENCIES + ament_cmake + std_msgs + rclcpp + rclcpp_action + ros_actor_action_server_msgs + ros_actor_message_queue_msgs +) + +foreach(dep IN LISTS DEPENDENCIES) + find_package(${dep} REQUIRED) +endforeach() + +include_directories( + ${ros_actor_message_queue_msgs_INCLUDE_DIRS} +) + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(ros_actor_action_server src/Server.cpp) +set_property(TARGET ros_actor_action_server PROPERTY CXX_STANDARD 17) +ament_target_dependencies(ros_actor_action_server ${DEPENDENCIES}) + +#add_executable(talker src/publisher_member_function.cpp) +#ament_target_dependencies(talker geometry_msgs rclcpp) +#add_executable(listener src/subscriber_member_function.cpp) +#ament_target_dependencies(listener geometry_msgs rclcpp) + +install(TARGETS + ros_actor_action_server + DESTINATION lib/${PROJECT_NAME}) + +if (BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif () + +ament_package() diff --git a/src/cpp_pubsub/package.xml b/src/ros_actor_action_server/package.xml similarity index 82% rename from src/cpp_pubsub/package.xml rename to src/ros_actor_action_server/package.xml index 44bd1f8..eea02dc 100644 --- a/src/cpp_pubsub/package.xml +++ b/src/ros_actor_action_server/package.xml @@ -1,7 +1,7 @@ - cpp_pubsub + ros_actor_action_server 0.0.0 TODO: Package description bastian @@ -13,6 +13,9 @@ ament_lint_auto ament_lint_common + ros_actor_action_server_msgs + ros_actor_message_queue_msgs + ament_cmake diff --git a/src/ros_actor_action_server/src/Server.cpp b/src/ros_actor_action_server/src/Server.cpp new file mode 100644 index 0000000..19d4353 --- /dev/null +++ b/src/ros_actor_action_server/src/Server.cpp @@ -0,0 +1,169 @@ +// +// Created by ros on 2/6/23. +// + +#include "Server.hpp" + +#include + +#include + +using namespace ros_actor_message_queue_msgs; + +void sendAction(mqd_t queue, ActionMessage *message) { mq_send(queue, (const char *)message, sizeof(ActionMessage), 0); } + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("ActorPlugin", "ActorPlugin"); + + node->get_node_options().arguments(); + + mq_attr queueAttributes; + queueAttributes.mq_flags = 0; + queueAttributes.mq_curmsgs = 0; + queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::FeedbackMessage); + queueAttributes.mq_maxmsg = 1; + mqd_t feedbackQueue = mq_open("/ActorPluginFeedback", O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes); + + if (feedbackQueue == (mqd_t) -1) { + perror(nullptr); + return -1; + } + + queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::ActionMessage); + mqd_t actionQueue = mq_open("/ActorPluginAction", O_CREAT | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes); + + if (actionQueue == (mqd_t) -1) { + perror(nullptr); + return -1; + } + + AnimationServerGoalPtr currentAnimationGoalPtr; + MovementServerGoalPtr currentMovementGoalPtr; + + double progress; + + FeedbackMessage currentFeedback; + ActionMessage currentAction; + std::mutex mutex; + + currentAction.state = IDLE; + + std::thread([feedbackQueue, ¤tAnimationGoalPtr, ¤tMovementGoalPtr, ¤tFeedback, ¤tAction, &mutex] { + while (true) { + FeedbackMessage newFeedback; + if(mq_receive(feedbackQueue, (char *)&newFeedback, sizeof(ros_actor_message_queue_msgs::FeedbackMessage), nullptr)==-1){ + perror(nullptr); + } + mutex.lock(); + if (newFeedback.state != currentFeedback.state) { + currentAction.state = newFeedback.state; + } + if(currentAction.state==ANIMATION && currentAnimationGoalPtr!=nullptr){ + if(newFeedback.progress<1){ + auto feedback = std::make_shared(); + feedback->progress = newFeedback.progress; + currentAnimationGoalPtr->publish_feedback(feedback); + }else{ + currentAnimationGoalPtr->succeed(std::make_shared()); + } + } + if(currentAction.state==MOVEMENT && currentMovementGoalPtr!=nullptr){ + if(newFeedback.progress<1){ + auto feedback = std::make_shared(); + feedback->progress = newFeedback.progress; + currentMovementGoalPtr->publish_feedback(feedback); + }else{ + currentMovementGoalPtr->succeed(std::make_shared()); + } + } + currentFeedback = newFeedback; + std::cout << "Got feedback, State: " << currentFeedback.state << " | Progress: " << currentFeedback.progress << std::endl; + mutex.unlock(); + } + }).detach(); + + auto animationServer = rclcpp_action::create_server( + node, "animation", + [¤tAction, &mutex](const rclcpp_action::GoalUUID goalUuid, const AnimationGoalPtr &animationGoal) { + // igndbg << "goal" << shared_mmap->currentState << std::endl; + mutex.lock(); + + if (currentAction.state == IDLE) { + std::cout << "Accepting..." << std::endl; + strcpy(currentAction.animationName, animationGoal->animation_name.data()); + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } else { + std::cout << "Rejecting..." << std::endl; + mutex.unlock(); + return rclcpp_action::GoalResponse::REJECT; + } + }, + [actionQueue, ¤tAction, &mutex](const AnimationServerGoalPtr &animationGoalPtr) { + if (currentAction.state == IDLE) { + return rclcpp_action::CancelResponse::REJECT; + } else { + mutex.lock(); + + currentAction.state = IDLE; + sendAction(actionQueue, ¤tAction); + + mutex.unlock(); + + return rclcpp_action::CancelResponse::ACCEPT; + } + }, + [actionQueue, ¤tAction, ¤tAnimationGoalPtr, &mutex](const AnimationServerGoalPtr &animationGoalPtr) { + currentAnimationGoalPtr = animationGoalPtr; + currentAction.state = ANIMATION; + sendAction(actionQueue, ¤tAction); + + mutex.unlock(); + }); + + auto movementServer = rclcpp_action::create_server( + node, "movement", + [¤tAction, &mutex](const rclcpp_action::GoalUUID goalUuid, const MovementGoalPtr &movementGoal) { + mutex.lock(); + if (currentAction.state == IDLE) { + std::cout << "Accepting..." << std::endl; + currentAction.positionX = movementGoal->target_position[0]; + currentAction.positionY = movementGoal->target_position[1]; + currentAction.positionZ = movementGoal->target_position[2]; + currentAction.animationDistance = movementGoal->animation_distance; + strcpy(currentAction.animationName, movementGoal->animation_name.data()); + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + } else { + std::cout << "Rejecting..." << std::endl; + mutex.unlock(); + return rclcpp_action::GoalResponse::REJECT; + } + }, + [actionQueue, ¤tAction, &mutex](const MovementServerGoalPtr &movementGoalPtr) { + if (currentAction.state == IDLE) { + return rclcpp_action::CancelResponse::REJECT; + } else { + mutex.lock(); + + currentAction.state = IDLE; + sendAction(actionQueue, ¤tAction); + + mutex.unlock(); + return rclcpp_action::CancelResponse::ACCEPT; + } + }, + [actionQueue, ¤tAction, ¤tMovementGoalPtr, &mutex](const MovementServerGoalPtr &movementGoalPtr) { + currentMovementGoalPtr = movementGoalPtr; + currentAction.state = MOVEMENT; + sendAction(actionQueue, ¤tAction); + + mutex.unlock(); + }); + + while(true){ + rclcpp::spin(node); + } +} \ No newline at end of file diff --git a/src/ros_actor_action_server/src/Server.hpp b/src/ros_actor_action_server/src/Server.hpp new file mode 100644 index 0000000..7d15a73 --- /dev/null +++ b/src/ros_actor_action_server/src/Server.hpp @@ -0,0 +1,29 @@ +// +// Created by ros on 2/6/23. +// + +#ifndef BUILD_SERVER_H +#define BUILD_SERVER_H + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace ros_actor_action_server_msgs; +using rclcpp_action::ServerGoalHandle; + +#define AnimationActionServer rclcpp_action::Server +#define MovementActionServer rclcpp_action::Server + +#define AnimationGoalPtr std::shared_ptr +#define AnimationServerGoalPtr std::shared_ptr> + +#define MovementGoalPtr std::shared_ptr +#define MovementServerGoalPtr std::shared_ptr> + +#endif //BUILD_SERVER_H diff --git a/src/ign_actor_plugin_msgs/CMakeLists.txt b/src/ros_actor_action_server_msgs/CMakeLists.txt similarity index 73% rename from src/ign_actor_plugin_msgs/CMakeLists.txt rename to src/ros_actor_action_server_msgs/CMakeLists.txt index f1e43dc..25628fa 100644 --- a/src/ign_actor_plugin_msgs/CMakeLists.txt +++ b/src/ros_actor_action_server_msgs/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) -project(ign_actor_plugin_msgs) +project(ros_actor_action_server_msgs) find_package(rosidl_default_generators REQUIRED) @@ -7,5 +7,6 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/Animation.action" "action/Movement.action" ) +ament_export_dependencies(rosidl_default_runtime) ament_package() diff --git a/src/ign_actor_plugin_msgs/action/Animation.action b/src/ros_actor_action_server_msgs/action/Animation.action similarity index 100% rename from src/ign_actor_plugin_msgs/action/Animation.action rename to src/ros_actor_action_server_msgs/action/Animation.action diff --git a/src/ign_actor_plugin_msgs/action/Movement.action b/src/ros_actor_action_server_msgs/action/Movement.action similarity index 100% rename from src/ign_actor_plugin_msgs/action/Movement.action rename to src/ros_actor_action_server_msgs/action/Movement.action diff --git a/src/ign_actor_plugin_msgs/package.xml b/src/ros_actor_action_server_msgs/package.xml similarity index 81% rename from src/ign_actor_plugin_msgs/package.xml rename to src/ros_actor_action_server_msgs/package.xml index 2b5168d..fd7b730 100644 --- a/src/ign_actor_plugin_msgs/package.xml +++ b/src/ros_actor_action_server_msgs/package.xml @@ -1,9 +1,9 @@ - ign_actor_plugin_msgs + ros_actor_action_server_msgs 0.0.0 - Plugin for Gazebo Ignition to remote control actors + ROS2 message definitions for ros_actor_action_server Bastian Hofmann TODO: License declaration diff --git a/src/ros_actor_message_queue_msgs/CMakeLists.txt b/src/ros_actor_message_queue_msgs/CMakeLists.txt new file mode 100644 index 0000000..d47c75a --- /dev/null +++ b/src/ros_actor_message_queue_msgs/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) +project(ros_actor_message_queue_msgs) + +find_package(ament_cmake REQUIRED) + +add_library(${PROJECT_NAME} INTERFACE) + +target_include_directories(${PROJECT_NAME} INTERFACE + "$" + "$" +) +target_link_libraries(${PROJECT_NAME} INTERFACE) + +install(TARGETS ${PROJECT_NAME} + EXPORT "export_${PROJECT_NAME}" + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib + INCLUDES DESTINATION include +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +ament_export_targets("export_${PROJECT_NAME}") + +ament_package() diff --git a/src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp b/src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp new file mode 100644 index 0000000..e8a8967 --- /dev/null +++ b/src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp @@ -0,0 +1,17 @@ +namespace ros_actor_message_queue_msgs{ +enum ActorPluginState{ + SETUP,IDLE,ANIMATION,MOVEMENT + }; + +struct FeedbackMessage{ + ActorPluginState state; + float progress; +}; + +struct ActionMessage{ + ActorPluginState state; + float positionX = 0.0f, positionY = 0.0f, positionZ = 0.0f; + float animationSpeed = 1.0f,animationDistance = 1.0f; + char animationName[256]; +}; +} \ No newline at end of file diff --git a/src/ros_actor_message_queue_msgs/package.xml b/src/ros_actor_message_queue_msgs/package.xml new file mode 100644 index 0000000..25048e9 --- /dev/null +++ b/src/ros_actor_message_queue_msgs/package.xml @@ -0,0 +1,15 @@ + + + + ros_actor_message_queue_msgs + 0.0.0 + POSIX message queue struct definitions for ros_actor_action_server + Bastian Hofmann + TODO: License declaration + + ament_cmake + + + ament_cmake + +