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

Final version of actor plugin

Browse Source
This commit is contained in:
Bastian Hofmann 2023-02-14 17:03:17 +00:00
parent bf36c02d2a
commit 17e61a505f
19 changed files with 579 additions and 548 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
src/cpp_pubsub/CMakeLists.txt
View File

@ -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(<dependency> 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()

52
src/cpp_pubsub/src/publisher_member_function.cpp
View File

@ -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 <chrono>
#include <memory>
#include <geometry_msgs/msg/pose.hpp>
#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<geometry_msgs::msg::Pose>("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<geometry_msgs::msg::Pose>::SharedPtr publisher_;
};
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MinimalPublisher>());
rclcpp::shutdown();
return 0;
}

66
src/cpp_pubsub/src/subscriber_member_function.cpp
View File

@ -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 <memory>
#include <geometry_msgs/msg/pose.hpp>
#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<geometry_msgs::msg::Pose>::SharedPtr subscription_ = this->create_subscription<geometry_msgs::msg::Pose>(
"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 <typename T>
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<T>(
topic_name, 10, [fun](const T result) {
fun(result);
});
}
private:
typename rclcpp::Subscription<T>::SharedPtr subscription_;
};
template <typename T>
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<MinimalSubscriber<T>>(node_name,topic_name,callback));
rclcpp::shutdown();
});
}
int main(int argc, char * argv[])
{
rclcpp::init(0, nullptr);
spinMinimalSubscriber<geometry_msgs::msg::Pose>("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;
}

1
src/gz_ros2_control

@ -1 +0,0 @@
Subproject commit b0c29c2f3245eca25753fbe2527eeacc7cb9dac0

27
src/ign_actor_plugin/CMakeLists.txt
View File

@ -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
ament_export_dependencies(ign_actor_plugin
"rosidl_default_runtime"
"ign_actor_plugin_msgs"
"ros_actor_action_server_msgs"
"rclcpp"
"rclcpp_action"
"rclcpp_components")
"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
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()

3
src/ign_actor_plugin/package.xml
View File

@ -11,7 +11,8 @@
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>rclcpp_components</depend>
<depend>ign_actor_plugin_msgs</depend>
<depend>ros_actor_message_queue_msgs</depend>
<depend>ros_actor_action_server_msgs</depend>
<depend>ignition-cmake2</depend>
<depend>ignition-gazebo6</depend>
<test_depend>ament_lint_auto</test_depend>

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

@ -2,42 +2,37 @@
// Created by bastian on 31.08.22.
//
#include "ActorSystem.hpp"
#include <chrono>
#include <ign_actor_plugin_msgs/action/detail/animation__struct.hpp>
#include <cstdio>
#include <fcntl.h>
#include <ignition/common/Console.hh>
#include <ignition/common/Mesh.hh>
#include <ignition/common/MeshManager.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/gazebo/Entity.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <ignition/gazebo/Types.hh>
#include <ignition/gazebo/components/Actor.hh>
#include <ignition/gazebo/components/Component.hh>
#include <ignition/gazebo/components/Pose.hh>
#include <rclcpp_action/create_server.hpp>
#include <rclcpp_action/server.hpp>
#include "ActorSystem.hpp"
#include <ignition/gazebo/components/Actor.hh>
#include <ignition/gazebo/EntityComponentManager.hh>
#include <sdf/Actor.hh>
#include <ignition/common/MeshManager.hh>
#include <ignition/common/SkeletonAnimation.hh>
#include <ignition/common/Mesh.hh>
#include <sys/mman.h>
#include <fcntl.h>
#include <cstdio>
#include <unistd.h>
IGNITION_ADD_PLUGIN(
ignition::gazebo::ActorSystem,
ignition::gazebo::System,
ignition::gazebo::ActorSystem::ISystemPreUpdate,
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<components::Actor>(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,29 +72,31 @@ 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<components::Pose>(entity);
auto newPose = initialPose;
newPose.Pos().X(0);
newPose.Pos().Y(0);
*poseComponent = components::Pose(newPose);
} else {
ignerr << "No animation found!" << std::endl;
}
}
void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) {
pthread_mutex_lock(&shared_mmap->mutex);
switch(shared_mmap->currentState){
threadMutex.lock();
switch (currentState) {
case SETUP: {
igndbg << "Setting up actor plugin" << std::endl;
@ -125,32 +122,29 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
_ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
}
shared_mmap->currentState = ActorSystemState::IDLE;
currentState = IDLE;
break;
}
case ANIMATION: {
if (shared_mmap->lastState == ActorSystemState::IDLE){
switchAnimation(_ecm, shared_mmap->animation_name);
if (lastState == IDLE) {
switchAnimation(_ecm, animation_name);
}
auto animationTimeComponent = _ecm.Component<components::AnimationTime>(this->entity);
auto newTime = animationTimeComponent->Data() + _info.dt;
auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
auto feedback = (float) (newTimeSeconds/duration);
write(feedback_pipe[1],&feedback,sizeof(float));
//currentAnimationGoalPtr->publish_feedback(feedback);
auto feedback = newTimeSeconds / duration;
sendFeedback(feedback);
if (newTimeSeconds >= duration){
//currentAnimationGoalPtr->succeed(std::make_shared<action::Animation::Result>());
igndbg << "Animation " << shared_mmap->animation_name << " finished." << std::endl;
shared_mmap->currentState = ActorSystemState::IDLE;
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;
@ -170,23 +164,21 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
break;
}
case MOVEMENT:{
if (shared_mmap->lastState == ActorSystemState::IDLE){
case MOVEMENT: {
if (lastState == IDLE) {
igndbg << "Starting Movement..." << std::endl;
switchAnimation(_ecm, shared_mmap->animation_name);
switchAnimation(_ecm, animation_name);
}
auto trajectoryPoseComp = _ecm.Component<components::TrajectoryPose>(this->entity);
auto actorPose = trajectoryPoseComp->Data();
auto targetPose = shared_mmap->target_position;
auto targetPose = target_position;
auto targetDirection = math::Vector3d(targetPose[0],targetPose[1],targetPose[2]) - actorPose.Pos();
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<action::Movement::Result>());
shared_mmap->currentState = ActorSystemState::IDLE;
if (targetDirection.Length() < 0.05) {
sendFeedback(1.0);
currentState = IDLE;
break;
}
@ -196,36 +188,37 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto newTimeSeconds = oldTimeSeconds + deltaTimeSeconds;
auto newTime = animationTimeComponent->Data() + _info.dt;
if(newTimeSeconds >= duration){
if (newTimeSeconds >= duration) {
newTimeSeconds -= duration;
newTime = std::chrono::duration_cast<std::chrono::steady_clock::duration>(std::chrono::duration<double>(newTimeSeconds));
}
auto targetYaw = atan2(targetDirection.Y(),targetDirection.X());
auto currentYaw = fmod(actorPose.Rot().Yaw(),M_PI);
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 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){
if (angularDirection < 0) {
turnSpeed = -turnSpeed;
}
if (abs(angularDirection) > 0.01){
actorPose.Rot().Euler(0,0,actorPose.Rot().Yaw()+(turnSpeed*deltaTimeSeconds));
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){
} 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;
double distance = (deltaTimeSeconds / duration) * animation_distance;
actorPose.Pos() += targetDirection.Normalize() * distance;
actorPose.Rot().Euler(0,0,targetYaw);
actorPose.Rot().Euler(0, 0, targetYaw);
*trajectoryPoseComp = components::TrajectoryPose(actorPose);
*animationTimeComponent = components::AnimationTime(newTime);
@ -235,159 +228,74 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
break;
}
case IDLE:
if (shared_mmap->lastState != shared_mmap->currentState){
igndbg << "Now idling..." << std::endl;
}
break;
}
shared_mmap->lastState = shared_mmap->currentState;
pthread_mutex_unlock(&shared_mmap->mutex);
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<action::Animation>(
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;
if (feedbackQueue == (mqd_t)-1) {
ignerr << "Could not create queue. (" << errno << ")" << std::endl;
return;
}
},
[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);
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);
return rclcpp_action::CancelResponse::ACCEPT;
if (actionQueue == (mqd_t)-1) {
ignerr << "Could not create queue. (" << errno << ")" << std::endl;
return;
}
},
[this,&currentAnimationGoalPtr](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<action::Movement>(
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,&currentMovementGoalPtr](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,&currentAnimationGoalPtr,&currentMovementGoalPtr]() {
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<action::Animation::Result>());
}
if(currentMovementGoalPtr!=nullptr){
currentMovementGoalPtr->succeed(std::make_shared<action::Movement::Result>());
}
}else{
if(currentAnimationGoalPtr!=nullptr){
auto feedback = std::make_shared<action::Animation::Feedback>();
feedback->set__progress(progress);
currentAnimationGoalPtr->publish_feedback(feedback);
}
if(currentMovementGoalPtr!=nullptr){
auto feedback = std::make_shared<action::Movement::Feedback>();
feedback->set__progress(progress);
currentMovementGoalPtr->publish_feedback(feedback);
}
}
usleep(500);
}
});
t.detach();
igndbg << "Spinning node..." << std::endl;
std::thread([this] {
ActionMessage receivedAction;
while (true) {
printf("Spinning...\n");
rclcpp::spin(node);
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<const sdf::Element> &_sdf,
EntityComponentManager &_ecm, EventManager &) {
void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, EntityComponentManager &_ecm,
EventManager &) {
igndbg << "actor plugin configuring..." << std::endl;
auto actorComp = _ecm.Component<components::Actor>(_entity);
@ -396,62 +304,20 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
return;
}
std::string topic = "ActorPlugin";
char topic[256] = "/ActorPlugin";
if (_sdf->HasElement("topic")) {
topic = _sdf->Get<std::string>("topic");
auto topicString = _sdf->Get<std::string>("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;
}

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

@ -14,14 +14,19 @@
#include <rclcpp/node.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/server.hpp>
#include <ign_actor_plugin_msgs/action/animation.hpp>
#include <ign_actor_plugin_msgs/action/movement.hpp>
#include <ros_actor_action_server_msgs/action/animation.hpp>
#include <ros_actor_action_server_msgs/action/movement.hpp>
#include <ros_actor_message_queue_msgs/MessageTypes.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <string>
#include <ignition/common/Skeleton.hh>
#include <ignition/common/NodeAnimation.hh>
#include <fcntl.h>
#include <sys/stat.h>
#include <mqueue.h>
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<action::Animation>
@ -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<common::Skeleton> currentSkeleton;
common::NodeAnimation* currentRootNodeAnimation;
math::Matrix4<double> 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);
};

57
src/ros_actor_action_server/CMakeLists.txt Normal file
View File

@ -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(<dependency> 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()

5
src/cpp_pubsub/package.xml → src/ros_actor_action_server/package.xml
View File

@ -1,7 +1,7 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cpp_pubsub</name>
<name>ros_actor_action_server</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="bastian@todo.todo">bastian</maintainer>
@ -13,6 +13,9 @@
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<depend>ros_actor_action_server_msgs</depend>
<depend>ros_actor_message_queue_msgs</depend>
<export>
<build_type>ament_cmake</build_type>
</export>

169
src/ros_actor_action_server/src/Server.cpp Normal file
View File

@ -0,0 +1,169 @@
//
// Created by ros on 2/6/23.
//
#include "Server.hpp"
#include <mqueue.h>
#include <cstdio>
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, &currentAnimationGoalPtr, &currentMovementGoalPtr, &currentFeedback, &currentAction, &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<ros_actor_action_server_msgs::action::Animation_Feedback>();
feedback->progress = newFeedback.progress;
currentAnimationGoalPtr->publish_feedback(feedback);
}else{
currentAnimationGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Animation_Result>());
}
}
if(currentAction.state==MOVEMENT && currentMovementGoalPtr!=nullptr){
if(newFeedback.progress<1){
auto feedback = std::make_shared<ros_actor_action_server_msgs::action::Movement_Feedback>();
feedback->progress = newFeedback.progress;
currentMovementGoalPtr->publish_feedback(feedback);
}else{
currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
}
}
currentFeedback = newFeedback;
std::cout << "Got feedback, State: " << currentFeedback.state << " | Progress: " << currentFeedback.progress << std::endl;
mutex.unlock();
}
}).detach();
auto animationServer = rclcpp_action::create_server<action::Animation>(
node, "animation",
[&currentAction, &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, &currentAction, &mutex](const AnimationServerGoalPtr &animationGoalPtr) {
if (currentAction.state == IDLE) {
return rclcpp_action::CancelResponse::REJECT;
} else {
mutex.lock();
currentAction.state = IDLE;
sendAction(actionQueue, &currentAction);
mutex.unlock();
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[actionQueue, &currentAction, &currentAnimationGoalPtr, &mutex](const AnimationServerGoalPtr &animationGoalPtr) {
currentAnimationGoalPtr = animationGoalPtr;
currentAction.state = ANIMATION;
sendAction(actionQueue, &currentAction);
mutex.unlock();
});
auto movementServer = rclcpp_action::create_server<action::Movement>(
node, "movement",
[&currentAction, &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, &currentAction, &mutex](const MovementServerGoalPtr &movementGoalPtr) {
if (currentAction.state == IDLE) {
return rclcpp_action::CancelResponse::REJECT;
} else {
mutex.lock();
currentAction.state = IDLE;
sendAction(actionQueue, &currentAction);
mutex.unlock();
return rclcpp_action::CancelResponse::ACCEPT;
}
},
[actionQueue, &currentAction, &currentMovementGoalPtr, &mutex](const MovementServerGoalPtr &movementGoalPtr) {
currentMovementGoalPtr = movementGoalPtr;
currentAction.state = MOVEMENT;
sendAction(actionQueue, &currentAction);
mutex.unlock();
});
while(true){
rclcpp::spin(node);
}
}

29
src/ros_actor_action_server/src/Server.hpp Normal file
View File

@ -0,0 +1,29 @@
//
// Created by ros on 2/6/23.
//
#ifndef BUILD_SERVER_H
#define BUILD_SERVER_H
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <ros_actor_action_server_msgs/action/animation.hpp>
#include <ros_actor_action_server_msgs/action/movement.hpp>
#include <ros_actor_message_queue_msgs/MessageTypes.hpp>
#include <fcntl.h>
#include <sys/stat.h>
#include <mqueue.h>
using namespace ros_actor_action_server_msgs;
using rclcpp_action::ServerGoalHandle;
#define AnimationActionServer rclcpp_action::Server<action::Animation>
#define MovementActionServer rclcpp_action::Server<action::Movement>
#define AnimationGoalPtr std::shared_ptr<const action::Animation::Goal>
#define AnimationServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Animation>>
#define MovementGoalPtr std::shared_ptr<const action::Movement::Goal>
#define MovementServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Movement>>
#endif //BUILD_SERVER_H

3
src/ign_actor_plugin_msgs/CMakeLists.txt → src/ros_actor_action_server_msgs/CMakeLists.txt
View File

@ -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()

0
src/ign_actor_plugin_msgs/action/Animation.action → src/ros_actor_action_server_msgs/action/Animation.action
View File

0
src/ign_actor_plugin_msgs/action/Movement.action → src/ros_actor_action_server_msgs/action/Movement.action
View File

4
src/ign_actor_plugin_msgs/package.xml → src/ros_actor_action_server_msgs/package.xml
View File

@ -1,9 +1,9 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ign_actor_plugin_msgs</name>
<name>ros_actor_action_server_msgs</name>
<version>0.0.0</version>
<description>Plugin for Gazebo Ignition to remote control actors</description>
<description>ROS2 message definitions for ros_actor_action_server</description>
<maintainer email="bastian@todo.todo">Bastian Hofmann</maintainer>
<license>TODO: License declaration</license>

28
src/ros_actor_message_queue_msgs/CMakeLists.txt Normal file
View File

@ -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
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include>"
)
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()

17
src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp Normal file
View File

@ -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];
};
}

15
src/ros_actor_message_queue_msgs/package.xml Normal file
View File

@ -0,0 +1,15 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>ros_actor_message_queue_msgs</name>
<version>0.0.0</version>
<description>POSIX message queue struct definitions for ros_actor_action_server</description>
<maintainer email="bastian@todo.todo">Bastian Hofmann</maintainer>
<license>TODO: License declaration</license>
<depend>ament_cmake</depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>