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

Changed message types and error handling to better suit behavior tree.

Browse Source
This commit is contained in:
Bastian Hofmann 2023-03-06 14:50:30 +00:00
parent d9074b1103
commit 67af90a280
21 changed files with 246 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/btree_nodes/CMakeLists.txt
View File

@ -12,10 +12,11 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
set(DEPENDENCIES
ament_cmake
rclcpp
rclcpp_action
#geometry_msgs
std_msgs
behaviortree_cpp_v3
rclcpp_action
tf2_geometry_msgs
moveit_core
moveit_ros_planning_interface

1
src/btree_nodes/package.xml
View File

@ -9,6 +9,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_action</depend>
<depend>geometry_msgs</depend>
<depend>ros_actor_action_server_msgs</depend>
<test_depend>ament_lint_auto</test_depend>

4
src/btree_nodes/src/Tree.cpp
View File

@ -82,7 +82,7 @@ int main(int argc, char **argv) {
factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
factory.registerNodeType<AmICalled>("AmICalled");
factory.registerNodeType<ActorMovement>("ActorMovement");
//factory.registerNodeType<ActorMovement>("ActorMovement");
factory.registerNodeType<MoveActorToTarget>("MoveActorToTarget");
factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
factory.registerNodeType<OffsetPose>("OffsetPose");
@ -104,7 +104,7 @@ int main(int argc, char **argv) {
factory.registerBuilder<SetRobotVelocity>("SetRobotVelocity", builderIisyVelocity);
bool called;
auto IsCalled = [&called](TreeNode& parent_node) -> NodeStatus{
auto IsCalled = [&called](__attribute__((unused)) TreeNode& parent_node) -> NodeStatus{
return called ? NodeStatus::SUCCESS : NodeStatus::FAILURE;
};

83
src/btree_nodes/src/nodes/ActorMovement.cpp
View File

@ -4,61 +4,68 @@
#include "ActorMovement.h"
BT::ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
: StatefulActionNode(name, config) {}
#include <memory>
BT::PortsList BT::ActorMovement::providedPorts() {
ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
: BT::StatefulActionNode(name, config) {
//this->client=client;
}
BT::PortsList ActorMovement::providedPorts() {
return {
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
BT::OutputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("current"),
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target")
};
}
BT::NodeStatus BT::ActorMovement::onStart() {
BT::NodeStatus ActorMovement::onStart() {
std::cout << "started moving" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
ros_actor_action_server_msgs::action::Movement::Goal goal;
auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
if (!res) {
std::cout << "[ no target available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
return BT::NodeStatus::FAILURE;
}
publisher->publish(*target);
return NodeStatus::RUNNING;
goal.animation_distance=1.5;
goal.animation_name="walk";
goal.animation_speed=1.0;
goal.target = *target;
auto send_goal_options = Client<Movement>::SendGoalOptions();
send_goal_options.feedback_callback = [=](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, const std::shared_ptr<const Movement::Feedback> feedback) {
setOutput("current",std::make_shared<geometry_msgs::msg::Pose>(feedback->current));
};
send_goal_options.result_callback = [=](const ClientGoalHandle<Movement>::WrappedResult & parameter) {
mutex.lock();
if(parameter.code == ResultCode::SUCCEEDED){
result = BT::NodeStatus::SUCCESS;
}else{
result = BT::NodeStatus::FAILURE;
}
mutex.unlock();
};
auto node = rclcpp::Node::make_shared("actorMovementPublisher");
client = rclcpp_action::create_client<Movement>(node,"/actorPlugin/movement");
client->async_send_goal(goal,send_goal_options);
return BT::NodeStatus::RUNNING;
}
BT::NodeStatus BT::ActorMovement::onRunning() {
std::shared_ptr<geometry_msgs::msg::Pose> current;
auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("current", current);
if (!res) {
std::cout << "[ no current position available ]" << std::endl;
std::cout << res.error() << std::endl;
return NodeStatus::FAILURE;
}
double dX = target->position.x - current->position.x;
double dY = target->position.y - current->position.y;
auto distance = sqrt(dX * dX + dY * dY);
if (distance < 0.3) {
return NodeStatus::SUCCESS;
} else {
return NodeStatus::RUNNING;
}
BT::NodeStatus ActorMovement::onRunning() {
mutex.lock();
auto status = result;
mutex.unlock();
return status;
}
void BT::ActorMovement::onHalted() {
void ActorMovement::onHalted() {
std::cout << "halted move" << std::endl;
rclcpp::Node node("targetPublisherNode");
auto publisher = node.create_publisher<geometry_msgs::msg::Pose>("targetPose", 10);
geometry_msgs::msg::Pose inf;
inf.position.x = HUGE_VAL;
inf.position.y = HUGE_VAL;
publisher->publish(inf);
client->async_cancel_all_goals();
}

31
src/btree_nodes/src/nodes/ActorMovement.h
View File

@ -6,25 +6,38 @@
#define BUILD_ACTORMOVEMENT_H
#include <behaviortree_cpp_v3/action_node.h>
#include <geometry_msgs/msg/pose.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_action/client_goal_handle.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <ros_actor_action_server_msgs/action/movement.hpp>
#include <memory>
#include <cstdlib>
namespace BT {
class ActorMovement : public StatefulActionNode {
using Movement = ros_actor_action_server_msgs::action::Movement;
using namespace rclcpp_action;
class ActorMovement : public BT::StatefulActionNode {
public:
ActorMovement(const std::string &name, const NodeConfiguration &config);
ActorMovement(const std::string &name, const BT::NodeConfiguration &config);
static PortsList providedPorts();
static BT::PortsList providedPorts();
std::shared_ptr<geometry_msgs::msg::Pose> target;
NodeStatus onStart() override;
BT::NodeStatus onStart() override;
NodeStatus onRunning() override;
BT::NodeStatus onRunning() override;
void onHalted() override;
};
}
private:
BT::NodeStatus result;
std::mutex mutex;
std::shared_ptr<Client<Movement>> client;
//rclcpp::Client<ros_actor_action_server_msgs::action::Movement>::FutureAndRequestId currentRequest;
};
#endif //BUILD_ACTORMOVEMENT_H

4
src/btree_nodes/src/nodes/MoveConnection.cpp
View File

@ -26,7 +26,7 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
std::cout<<"Parameters set."<<std::endl;
moveit::planning_interface::MoveGroupInterface::Plan plan;
if(moveGroup.plan(plan)==moveit::planning_interface::MoveItErrorCode::SUCCESS){
if(moveGroup.plan(plan)==moveit::core::MoveItErrorCode::SUCCESS){
std::cout<<"Planned a path."<<std::endl;
}else{
std::cout<<"Error during planning."<<std::endl;
@ -38,7 +38,7 @@ void MoveConnection::planAndExecute(const std::shared_ptr<geometry_msgs::msg::Po
lock.unlock();
return;
}
bool success = moveGroup.execute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS;
bool success = moveGroup.execute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
if(!cancelled) {
callback(success);
}

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

@ -97,7 +97,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
if(nextState != currentState){
igndbg << "State change before: " << currentState << " -> " << nextState << std::endl;
currentState = nextState;
sendFeedback(0.0);
feedback.progress = 0.0f;
sendFeedback();
}
switch (currentState) {
@ -123,7 +124,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto trajectoryPoseComponent = _ecm.Component<components::TrajectoryPose>(entity);
if (nullptr == trajectoryPoseComponent) {
_ecm.CreateComponent(entity, components::TrajectoryPose(ignition::math::Pose3d::Zero));
_ecm.CreateComponent(entity, components::TrajectoryPose(startPose));
}
nextState = IDLE;
@ -137,13 +138,14 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
auto newTime = animationTimeComponent->Data() + _info.dt;
auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
auto feedback = newTimeSeconds / duration;
sendFeedback(feedback);
feedback.progress = newTimeSeconds / duration;
sendFeedback();
if (newTimeSeconds >= duration) {
igndbg << "Animation " << animation_name << " finished." << std::endl;
nextState = IDLE;
sendFeedback(0.0);
feedback.progress = 0.0f;
sendFeedback();
break;
}
@ -183,7 +185,8 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
if (targetDirection.Length() < 0.05) {
igndbg << "Finished Movement..." << std::endl;
sendFeedback(1.0);
feedback.progress = 1.0f;
sendFeedback();
nextState = IDLE;
break;
}
@ -212,6 +215,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
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 {
@ -243,55 +247,72 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
}
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);
if (feedbackQueue == (mqd_t)-1) {
ignerr << "Could not create queue. (" << errno << ")" << std::endl;
return;
igndbg << "Waiting for " << generatedName << std::endl;
feedbackQueue = (mqd_t) -1;
while(feedbackQueue == (mqd_t) -1){
feedbackQueue = mq_open(generatedName, O_RDWR);
if (feedbackQueue == (mqd_t) -1) {
if(errno!=ENOENT){
perror(nullptr);
return;
}
mq_close(feedbackQueue);
sleep(100);
}
}
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;
igndbg << "Waiting for " << generatedName << std::endl;
actionQueue = (mqd_t) -1;
while(actionQueue == (mqd_t) -1){
actionQueue = mq_open(generatedName, O_RDWR);
if (actionQueue == (mqd_t) -1) {
if(errno!=ENOENT){
perror(nullptr);
return;
}
mq_close(actionQueue);
sleep(100);
}
}
igndbg << "Queues online, starting listener" << std::endl;
std::thread([this] {
ActionMessage receivedAction;
while (true) {
igndbg << "Waiting for action" << std::endl;
mq_receive(actionQueue, (char *)&receivedAction, sizeof(ActionMessage), nullptr);
igndbg << "Got Action" << std::endl;
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);
target_position = math::Vector3d(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
nextState = receivedAction.state;
std::cout << "Got Action" << std::endl;
threadMutex.unlock();
}
}).detach();
}
void ActorSystem::sendFeedback(double progress){
FeedbackMessage message;
message.progress = progress;
message.state = currentState;
if(mq_send(feedbackQueue,(char *)&message,sizeof(FeedbackMessage),0)==0){
igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< progress <<")" << std::endl;
void ActorSystem::sendFeedback(){
feedback.state = currentState;
if(mq_send(feedbackQueue,(char *)&feedback,sizeof(FeedbackMessage),0)==0){
igndbg << "Sent feedback. (State: " << lastState << "->" << currentState << " | Progress: "<< feedback.progress <<")" << std::endl;
}else{
ignerr << "Error " << errno << " sending feedback." << std::endl;
}
@ -320,6 +341,21 @@ void ActorSystem::Configure(const Entity &_entity, const std::shared_ptr<const s
strcpy(topic, topicString.c_str());
}
if (_sdf->HasElement("x")) {
auto x = _sdf->Get<float>("x");
startPose.SetX(x);
}
if (_sdf->HasElement("y")) {
auto y = _sdf->Get<float>("y");
startPose.SetY(y);
}
if (_sdf->HasElement("rot")) {
auto rot = _sdf->Get<float>("rot");
startPose.Rot().Euler(0,0,rot);
}
this->entity = _entity;
messageQueueInterface(topic);

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

@ -39,55 +39,58 @@ using rclcpp_action::ServerGoalHandle;
#define MovementServerGoalPtr std::shared_ptr<ServerGoalHandle<action::Movement>>
namespace ignition {
namespace gazebo {
namespace gazebo {
class ActorSystem :
class ActorSystem :
public System,
public ISystemPreUpdate,
public ISystemConfigure {
public ISystemPreUpdate,
public ISystemConfigure {
private:
//std::shared_ptr<rclcpp::Node> node;
//std::shared_ptr<AnimationActionServer> animationServer;
//std::shared_ptr<MovementActionServer> movementServer;
//ActorSystemState currentState, lastState;
//action::Animation::Goal animationTarget;
//action::Movement::Goal movementTarget;
//AnimationServerGoalPtr currentAnimationGoalPtr;
//MovementServerGoalPtr currentMovementGoalPtr;
math::Vector3d target_position;
double animation_distance;
char animation_name[256];
ActorPluginState nextState,currentState,lastState;
double duration{};
Entity entity{kNullEntity};
std::mutex threadMutex;
std::shared_ptr<common::Skeleton> currentSkeleton;
common::NodeAnimation* currentRootNodeAnimation;
math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
mqd_t feedbackQueue;
mqd_t actionQueue;
private:
//std::shared_ptr<rclcpp::Node> node;
//std::shared_ptr<AnimationActionServer> animationServer;
//std::shared_ptr<MovementActionServer> movementServer;
//ActorSystemState currentState, lastState;
//action::Animation::Goal animationTarget;
//action::Movement::Goal movementTarget;
//AnimationServerGoalPtr currentAnimationGoalPtr;
//MovementServerGoalPtr currentMovementGoalPtr;
ignition::math::v6::Pose3d startPose = ignition::math::Pose3d::Zero;
math::Vector3d target_position;
double animation_distance;
char animation_name[256];
ActorPluginState nextState,currentState,lastState;
double duration{};
Entity entity{kNullEntity};
std::mutex threadMutex;
std::shared_ptr<common::Skeleton> currentSkeleton;
common::NodeAnimation* currentRootNodeAnimation;
math::Matrix4<double> currentRotationAlign, currentTranslationAlign;
mqd_t feedbackQueue;
mqd_t actionQueue;
FeedbackMessage feedback;
public:
ActorSystem();
public:
ActorSystem();
public:
~ActorSystem() override;
public:
~ActorSystem() override;
public:
void PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm) override;
public:
void PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm) override;
public:
void Configure(const ignition::gazebo::Entity &animationGoalPtr,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/) override;
public:
void Configure(const ignition::gazebo::Entity &animationGoalPtr,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/) override;
private:
void switchAnimation(EntityComponentManager &_ecm, char *animationName);
void messageQueueInterface(const char name[256]);
void sendFeedback(double feedback);
private:
void switchAnimation(EntityComponentManager &_ecm, char *animationName);
void messageQueueInterface(const char name[256]);
void sendFeedback();
void addPoseToFeedback(ignition::math::Pose3<double> pose);
};

3
src/ign_world/world/gaz_new_test.sdf
View File

@ -25,6 +25,9 @@
<actor name="actor_walking">
<plugin name="ignition::gazebo::ActorSystem" filename="/home/ros/workspace/install/ign_actor_plugin/lib/ign_actor_plugin/libign_actor_plugin.so">
<x>2</x>
<y>2</y>
<rot>180</rot>
</plugin>
<skin>
<filename>/home/ros/walk.dae</filename>

14
src/iisy_config_2/config/iisy.urdf
View File

@ -31,7 +31,7 @@
</visual>
<inertial>
<origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/>
<mass value="6"/>
<mass value="0.1"/>
<inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
</inertial>
</link>
@ -49,7 +49,7 @@
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.005 0.08"/>
<mass value="2"/>
<mass value="0.1"/>
<inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
</inertial>
</link>
@ -66,7 +66,7 @@
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/>
<mass value="4"/>
<mass value="0.1"/>
<inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
</inertial>
</link>
@ -84,7 +84,7 @@
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0.07 0.025"/>
<mass value="2"/>
<mass value="0.1"/>
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
</inertial>
</link>
@ -101,7 +101,7 @@
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 0.037363 0.086674"/>
<mass value="2"/>
<mass value="0.1"/>
<inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
</inertial>
</link>
@ -118,7 +118,7 @@
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/>
<mass value="2"/>
<mass value="0.1"/>
<inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
</inertial>
</link>
@ -135,7 +135,7 @@
</visual>
<inertial>
<origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/>
<mass value="0.8"/>
<mass value="0.1"/>
<inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
</inertial>
</link>

4
src/iisy_config_2/config/moveit_controllers.yaml
View File

@ -1,6 +1,8 @@
# MoveIt uses this configuration for controller management
trajectory_execution:
allowed_execution_duration_scaling: 1.2 # Wait for moves to finish without timeout
trajectory_duration_monitoring: false
allowed_execution_duration_scaling: 2.0 # Wait for moves to finish without timeout
allowed_goal_duration_margin: 2.0
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager

8
src/iisy_config_2/config/ros2_controllers.yaml
View File

@ -1,7 +1,7 @@
# This config file is used by ros2_control
controller_manager:
ros__parameters:
update_rate: 100 # Hz
update_rate: 1000 # Hz
arm_controller:
type: joint_trajectory_controller/JointTrajectoryController
@ -23,4 +23,8 @@ arm_controller:
- position
state_interfaces:
- position
- velocity
- velocity
open_loop_control: true
constraints:
stopped_velocity_tolerance: 0.01
goal_time: 0.0

2
src/ros_actor_action_server/package.xml
View File

@ -12,7 +12,7 @@
<depend>geometry_msgs</depend>
<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>

45
src/ros_actor_action_server/src/Server.cpp
View File

@ -7,10 +7,15 @@
#include <mqueue.h>
#include <cstdio>
#include <rclcpp/logger.hpp>
using namespace ros_actor_message_queue_msgs;
void sendAction(mqd_t queue, ActionMessage *message) { mq_send(queue, (const char *)message, sizeof(ActionMessage), 0); }
void sendAction(mqd_t queue, ActionMessage *message) {
std::cout << "Sending action..." << std::endl;
mq_send(queue, (const char *)message, sizeof(ActionMessage), 0);
std::cout << "Sent action" << std::endl;
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
@ -19,31 +24,45 @@ int main(int argc, char **argv) {
node->get_node_options().arguments();
feedbackName = "/ActorPluginFeedback";
actionName = "/ActorPluginAction";
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) {
mq_unlink(feedbackName);
queueAttributes.mq_msgsize = sizeof(ros_actor_message_queue_msgs::FeedbackMessage);
mqd_t feedbackQueue = mq_open(feedbackName, O_CREAT | O_EXCL | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
if(feedbackQueue == (mqd_t) -1){
perror(nullptr);
return -1;
}
std::atexit([]{
std::cout << "Shutting down " << feedbackName << std::endl;
mq_unlink(feedbackName);
});
mq_unlink(actionName);
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) {
mqd_t actionQueue = mq_open(actionName, O_CREAT | O_EXCL | O_RDWR, S_IRWXU | S_IRWXG, &queueAttributes);
if(actionQueue == (mqd_t) -1){
perror(nullptr);
return -1;
}
std::atexit([]{
std::cout << "Shutting down " << actionName << std::endl;
mq_unlink(actionName);
});
std::cout << "All message queues online." << std::endl;
AnimationServerGoalPtr currentAnimationGoalPtr;
MovementServerGoalPtr currentMovementGoalPtr;
double progress;
FeedbackMessage currentFeedback;
ActionMessage currentAction;
std::mutex mutex;
@ -72,7 +91,7 @@ int main(int argc, char **argv) {
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;
feedback->current = newFeedback.current;
currentMovementGoalPtr->publish_feedback(feedback);
}else{
currentMovementGoalPtr->succeed(std::make_shared<ros_actor_action_server_msgs::action::Movement_Result>());
@ -129,9 +148,7 @@ int main(int argc, char **argv) {
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.target = movementGoal->target;
currentAction.animationDistance = movementGoal->animation_distance;
strcpy(currentAction.animationName, movementGoal->animation_name.data());
@ -163,7 +180,7 @@ int main(int argc, char **argv) {
mutex.unlock();
});
while(true){
while(rclcpp::ok()){
rclcpp::spin(node);
}
}

3
src/ros_actor_action_server/src/Server.hpp
View File

@ -17,6 +17,9 @@
using namespace ros_actor_action_server_msgs;
using rclcpp_action::ServerGoalHandle;
static char *feedbackName;
static char *actionName;
#define AnimationActionServer rclcpp_action::Server<action::Animation>
#define MovementActionServer rclcpp_action::Server<action::Movement>

2
src/ros_actor_action_server_msgs/CMakeLists.txt
View File

@ -2,10 +2,12 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
project(ros_actor_action_server_msgs)
find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/Animation.action"
"action/Movement.action"
DEPENDENCIES geometry_msgs
)
ament_export_dependencies(rosidl_default_runtime)

5
src/ros_actor_action_server_msgs/action/Movement.action
View File

@ -1,8 +1,7 @@
string animation_name
float32 animation_speed
float32 animation_distance
float32[3] target_position
float32[3] target_orientation
geometry_msgs/Pose target
---
---
float32 progress
geometry_msgs/Pose current

1
src/ros_actor_action_server_msgs/package.xml
View File

@ -9,6 +9,7 @@
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<depend>geometry_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>

1
src/ros_actor_message_queue_msgs/CMakeLists.txt
View File

@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
project(ros_actor_message_queue_msgs)
find_package(ament_cmake REQUIRED)
find_package(geometry_msgs REQUIRED)
add_library(${PROJECT_NAME} INTERFACE)

4
src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp
View File

@ -1,3 +1,4 @@
#include <geometry_msgs/msg/detail/pose__struct.hpp>
namespace ros_actor_message_queue_msgs{
enum ActorPluginState{
SETUP,IDLE,ANIMATION,MOVEMENT
@ -6,11 +7,12 @@ enum ActorPluginState{
struct FeedbackMessage{
ActorPluginState state;
float progress;
geometry_msgs::msg::Pose current;
};
struct ActionMessage{
ActorPluginState state;
float positionX = 0.0f, positionY = 0.0f, positionZ = 0.0f;
geometry_msgs::msg::Pose target;
float animationSpeed = 1.0f,animationDistance = 1.0f;
char animationName[256];
};

2
test.sh
View File

@ -1,2 +1,2 @@
ros2 action send_goal /ActorPlugin/animation ign_actor_plugin_msgs/action/Animation "{animation_name: 'walk', animation_speed: 1.0}"
ros2 action send_goal /ActorPlugin/movement ign_actor_plugin_msgs/action/Movement "{animation_name: 'walk', animation_speed: 1.0, animation_distance: 1.5, target_position: [1,1,0], target_orientation: [0,0,0]}"
ros2 action send_goal /ActorPlugin/movement ros_actor_action_server_msgs/action/Movement '{animation_name: "walk", animation_speed: 1.0, animation_distance: 1.5, target: {orientation: {x: 0.0, y: 0.0, z: 0.0, w: 0.0}, position: {x: 2.0, y: 0.0, z: 0.0}}}'