diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt
index a268ef5..2b87213 100644
--- a/src/btree_nodes/CMakeLists.txt
+++ b/src/btree_nodes/CMakeLists.txt
@@ -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
diff --git a/src/btree_nodes/package.xml b/src/btree_nodes/package.xml
index 5424bb2..9b68e79 100644
--- a/src/btree_nodes/package.xml
+++ b/src/btree_nodes/package.xml
@@ -9,6 +9,7 @@
ament_cmake
rclcpp
+ rclcpp_action
geometry_msgs
ros_actor_action_server_msgs
ament_lint_auto
diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp
index 83dc29a..8fc4580 100644
--- a/src/btree_nodes/src/Tree.cpp
+++ b/src/btree_nodes/src/Tree.cpp
@@ -82,7 +82,7 @@ int main(int argc, char **argv) {
factory.registerNodeType("GenerateXYPose");
factory.registerNodeType("AmICalled");
- factory.registerNodeType("ActorMovement");
+ //factory.registerNodeType("ActorMovement");
factory.registerNodeType("MoveActorToTarget");
factory.registerNodeType("WeightedRandom");
factory.registerNodeType("OffsetPose");
@@ -104,7 +104,7 @@ int main(int argc, char **argv) {
factory.registerBuilder("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;
};
diff --git a/src/btree_nodes/src/nodes/ActorMovement.cpp b/src/btree_nodes/src/nodes/ActorMovement.cpp
index 89f459e..f1e6945 100644
--- a/src/btree_nodes/src/nodes/ActorMovement.cpp
+++ b/src/btree_nodes/src/nodes/ActorMovement.cpp
@@ -4,61 +4,68 @@
#include "ActorMovement.h"
-BT::ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguration &config)
- : StatefulActionNode(name, config) {}
+#include
-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>("current"),
- InputPort>("target")
+ BT::OutputPort>("current"),
+ BT::InputPort>("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("targetPose", 10);
+ ros_actor_action_server_msgs::action::Movement::Goal goal;
+
auto res = getInput>("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::SendGoalOptions();
+ send_goal_options.feedback_callback = [=](__attribute__((unused)) std::shared_ptr> goal_handle, const std::shared_ptr feedback) {
+ setOutput("current",std::make_shared(feedback->current));
+ };
+ send_goal_options.result_callback = [=](const ClientGoalHandle::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(node,"/actorPlugin/movement");
+ client->async_send_goal(goal,send_goal_options);
+
+ return BT::NodeStatus::RUNNING;
}
-BT::NodeStatus BT::ActorMovement::onRunning() {
- std::shared_ptr current;
-
- auto res = getInput>("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("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();
}
diff --git a/src/btree_nodes/src/nodes/ActorMovement.h b/src/btree_nodes/src/nodes/ActorMovement.h
index bdbaa04..8b48279 100644
--- a/src/btree_nodes/src/nodes/ActorMovement.h
+++ b/src/btree_nodes/src/nodes/ActorMovement.h
@@ -6,25 +6,38 @@
#define BUILD_ACTORMOVEMENT_H
#include
-#include
+#include
#include
+#include
+#include
+#include
+#include
+#include
+#include
-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 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;
+ //rclcpp::Client::FutureAndRequestId currentRequest;
+};
#endif //BUILD_ACTORMOVEMENT_H
diff --git a/src/btree_nodes/src/nodes/MoveConnection.cpp b/src/btree_nodes/src/nodes/MoveConnection.cpp
index 8460628..eb076f6 100644
--- a/src/btree_nodes/src/nodes/MoveConnection.cpp
+++ b/src/btree_nodes/src/nodes/MoveConnection.cpp
@@ -26,7 +26,7 @@ void MoveConnection::planAndExecute(const std::shared_ptr " << 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(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(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_ptrHasElement("x")) {
+ auto x = _sdf->Get("x");
+ startPose.SetX(x);
+ }
+
+ if (_sdf->HasElement("y")) {
+ auto y = _sdf->Get("y");
+ startPose.SetY(y);
+ }
+
+ if (_sdf->HasElement("rot")) {
+ auto rot = _sdf->Get("rot");
+ startPose.Rot().Euler(0,0,rot);
+ }
+
this->entity = _entity;
messageQueueInterface(topic);
diff --git a/src/ign_actor_plugin/src/ActorSystem.hpp b/src/ign_actor_plugin/src/ActorSystem.hpp
index 030a91f..329dea1 100644
--- a/src/ign_actor_plugin/src/ActorSystem.hpp
+++ b/src/ign_actor_plugin/src/ActorSystem.hpp
@@ -39,55 +39,58 @@ using rclcpp_action::ServerGoalHandle;
#define MovementServerGoalPtr std::shared_ptr>
namespace ignition {
- namespace gazebo {
+namespace gazebo {
- class ActorSystem :
+class ActorSystem :
public System,
- public ISystemPreUpdate,
- public ISystemConfigure {
+ public ISystemPreUpdate,
+ public ISystemConfigure {
- private:
- //std::shared_ptr node;
- //std::shared_ptr animationServer;
- //std::shared_ptr 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 currentSkeleton;
- common::NodeAnimation* currentRootNodeAnimation;
- math::Matrix4 currentRotationAlign, currentTranslationAlign;
- mqd_t feedbackQueue;
- mqd_t actionQueue;
+ private:
+ //std::shared_ptr node;
+ //std::shared_ptr animationServer;
+ //std::shared_ptr 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 currentSkeleton;
+ common::NodeAnimation* currentRootNodeAnimation;
+ math::Matrix4 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 &_sdf,
- EntityComponentManager &_ecm,
- EventManager &/*_eventMgr*/) override;
+ public:
+ void Configure(const ignition::gazebo::Entity &animationGoalPtr,
+ const std::shared_ptr &_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 pose);
};
diff --git a/src/ign_world/world/gaz_new_test.sdf b/src/ign_world/world/gaz_new_test.sdf
index a1a1816..291f97c 100644
--- a/src/ign_world/world/gaz_new_test.sdf
+++ b/src/ign_world/world/gaz_new_test.sdf
@@ -25,6 +25,9 @@
+ 2
+ 2
+ 180
/home/ros/walk.dae
diff --git a/src/iisy_config_2/config/iisy.urdf b/src/iisy_config_2/config/iisy.urdf
index feb1072..8508066 100644
--- a/src/iisy_config_2/config/iisy.urdf
+++ b/src/iisy_config_2/config/iisy.urdf
@@ -31,7 +31,7 @@
-
+
@@ -49,7 +49,7 @@
-
+
@@ -66,7 +66,7 @@
-
+
@@ -84,7 +84,7 @@
-
+
@@ -101,7 +101,7 @@
-
+
@@ -118,7 +118,7 @@
-
+
@@ -135,7 +135,7 @@
-
+
diff --git a/src/iisy_config_2/config/moveit_controllers.yaml b/src/iisy_config_2/config/moveit_controllers.yaml
index e278a35..1491116 100644
--- a/src/iisy_config_2/config/moveit_controllers.yaml
+++ b/src/iisy_config_2/config/moveit_controllers.yaml
@@ -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
diff --git a/src/iisy_config_2/config/ros2_controllers.yaml b/src/iisy_config_2/config/ros2_controllers.yaml
index f919fac..1e794bf 100644
--- a/src/iisy_config_2/config/ros2_controllers.yaml
+++ b/src/iisy_config_2/config/ros2_controllers.yaml
@@ -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
\ No newline at end of file
+ - velocity
+ open_loop_control: true
+ constraints:
+ stopped_velocity_tolerance: 0.01
+ goal_time: 0.0
\ No newline at end of file
diff --git a/src/ros_actor_action_server/package.xml b/src/ros_actor_action_server/package.xml
index eea02dc..c8408ea 100644
--- a/src/ros_actor_action_server/package.xml
+++ b/src/ros_actor_action_server/package.xml
@@ -12,7 +12,7 @@
geometry_msgs
ament_lint_auto
ament_lint_common
-
+
ros_actor_action_server_msgs
ros_actor_message_queue_msgs
diff --git a/src/ros_actor_action_server/src/Server.cpp b/src/ros_actor_action_server/src/Server.cpp
index 19d4353..92df04b 100644
--- a/src/ros_actor_action_server/src/Server.cpp
+++ b/src/ros_actor_action_server/src/Server.cpp
@@ -7,10 +7,15 @@
#include
#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); }
+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();
- feedback->progress = newFeedback.progress;
+ feedback->current = newFeedback.current;
currentMovementGoalPtr->publish_feedback(feedback);
}else{
currentMovementGoalPtr->succeed(std::make_shared());
@@ -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);
}
}
\ 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
index 7d15a73..4f8295b 100644
--- a/src/ros_actor_action_server/src/Server.hpp
+++ b/src/ros_actor_action_server/src/Server.hpp
@@ -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
#define MovementActionServer rclcpp_action::Server
diff --git a/src/ros_actor_action_server_msgs/CMakeLists.txt b/src/ros_actor_action_server_msgs/CMakeLists.txt
index 25628fa..ebc48b1 100644
--- a/src/ros_actor_action_server_msgs/CMakeLists.txt
+++ b/src/ros_actor_action_server_msgs/CMakeLists.txt
@@ -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)
diff --git a/src/ros_actor_action_server_msgs/action/Movement.action b/src/ros_actor_action_server_msgs/action/Movement.action
index 9d75095..0a750f3 100644
--- a/src/ros_actor_action_server_msgs/action/Movement.action
+++ b/src/ros_actor_action_server_msgs/action/Movement.action
@@ -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
diff --git a/src/ros_actor_action_server_msgs/package.xml b/src/ros_actor_action_server_msgs/package.xml
index fd7b730..06b7fc2 100644
--- a/src/ros_actor_action_server_msgs/package.xml
+++ b/src/ros_actor_action_server_msgs/package.xml
@@ -9,6 +9,7 @@
rosidl_default_generators
action_msgs
+ geometry_msgs
rosidl_interface_packages
diff --git a/src/ros_actor_message_queue_msgs/CMakeLists.txt b/src/ros_actor_message_queue_msgs/CMakeLists.txt
index d47c75a..4c8eab8 100644
--- a/src/ros_actor_message_queue_msgs/CMakeLists.txt
+++ b/src/ros_actor_message_queue_msgs/CMakeLists.txt
@@ -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)
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
index e8a8967..bd9be93 100644
--- 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
@@ -1,3 +1,4 @@
+#include
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];
};
diff --git a/test.sh b/test.sh
index 260380d..afbf897 100755
--- a/test.sh
+++ b/test.sh
@@ -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}}}'
\ No newline at end of file