minor adjustments all over the place.

src/btree/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
-project(btree_nodes)
+project(btree)
 
 add_compile_options(-Wall -Wextra -Wpedantic)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
@@ -38,7 +38,6 @@ set(CPP_FILES
         src/Tree.cpp
         src/Extensions.cpp
         src/nodes/WeightedRandomNode.cpp
-        src/nodes/AmICalled.cpp
         src/nodes/GenerateXYPose.cpp
         src/nodes/RobotMove.cpp
         src/nodes/MoveConnection.cpp
@@ -59,6 +58,8 @@ install(TARGETS
         tree
         DESTINATION lib/${PROJECT_NAME})
         
+install(DIRECTORY   trees     DESTINATION   share/${PROJECT_NAME})
+
 if (BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)
     # the following line skips the linter which checks for copyrights

src/btree/nodes.xml

@@ -70,7 +70,7 @@
         </Action>
 
         <Action ID="RandomFailure">
-            <input_port name="failureChance">Chance to fail from 0 to 1</input_port>
+            <input_port name="failure_chance">Chance to fail from 0 to 1</input_port>
         </Action>
     </TreeNodesModel>
 </root>

src/btree/package.xml

@@ -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>btree_nodes</name>
+  <name>btree</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="bastian@todo.todo">bastian</maintainer>

src/btree/src/Tree.cpp

@@ -8,7 +8,6 @@
 #include <rclcpp/utilities.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include "nodes/WeightedRandomNode.h"
-#include "nodes/AmICalled.h"
 #include "nodes/GenerateXYPose.h"
 #include "nodes/RobotMove.h"
 #include "nodes/OffsetPose.h"
@@ -40,7 +39,6 @@ int main(int argc, char **argv) {
     std::cout << "Registering nodes." << std::endl;
 
     factory.registerNodeType<GenerateXYPose>("GenerateXYPose");
-    factory.registerNodeType<AmICalled>("AmICalled");
 
     factory.registerNodeType<WeightedRandomNode>("WeightedRandom");
     factory.registerNodeType<OffsetPose>("OffsetPose");

src/btree/src/nodes/ActorAnimation.cpp

@@ -12,7 +12,8 @@ ActorAnimation::ActorAnimation(const std::string &name, const BT::NodeConfigurat
 
 BT::PortsList ActorAnimation::providedPorts() {
     return {
-        BT::InputPort<std::string>("animation"),
+        BT::InputPort<std::string>("animation_name"),
+        BT::InputPort<float>("animation_speed"),
     };
 }
 
@@ -21,15 +22,16 @@ BT::NodeStatus ActorAnimation::onStart() {
 
     ros_actor_action_server_msgs::action::Animation::Goal goal;
 
-    auto res = getInput<std::string>("animation", goal.animation_name);
+    if (!getInput<std::string>("animation_name", goal.animation_name)) {
-
-    if (!res) {
         std::cerr << "[ no animation name specified ]" << std::endl;
-        std::cout << res.error() << std::endl;
         return BT::NodeStatus::FAILURE;
     }
     
-    goal.animation_speed=1.0;
+    if (!getInput<float>("animation_speed", goal.animation_speed)) {
+        goal.animation_speed=1.0;
+    }
+
+    
 
     auto send_goal_options = Client<Animation>::SendGoalOptions();
     send_goal_options.result_callback = [=](const ClientGoalHandle<Animation>::WrappedResult & parameter) {

src/btree/src/nodes/ActorMovement.cpp

@@ -16,7 +16,8 @@ ActorMovement::ActorMovement(const std::string &name, const BT::NodeConfiguratio
 BT::PortsList ActorMovement::providedPorts() {
     return {
         BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("target"),
-        BT::InputPort<std::string>("animation"),
+        BT::InputPort<std::string>("animation_name"),
+        BT::InputPort<float>("animation_distance"),
     };
 }
 
@@ -25,19 +26,23 @@ BT::NodeStatus ActorMovement::onStart() {
 
     ros_actor_action_server_msgs::action::Movement::Goal goal;
 
-    std::shared_ptr<geometry_msgs::msg::Pose> target;
+    {
-    auto res = getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target);
+        std::shared_ptr<geometry_msgs::msg::Pose> target;
-
+        if (!getInput<std::shared_ptr<geometry_msgs::msg::Pose>>("target", target)) {
-    if (!res) {
+            std::cout << "[ no target given ]" << std::endl;
-        std::cout << "[ no target available ]" << std::endl;
+            return BT::NodeStatus::FAILURE;
-        std::cout << res.error() << std::endl;
+        }
-        return BT::NodeStatus::FAILURE;
+        goal.target = *target;
     }
     
-    goal.animation_distance=1.5;
+    if(!getInput<float>("animation_distance", goal.animation_distance)){
-    goal.animation_name="walk";
+        goal.animation_distance = 1.0f;
-    goal.animation_speed=1.0;
+    }
-    goal.target = *target;
+    
+    if(!getInput<std::string>("animation_name", goal.animation_name)){
+        std::cout << "[ no animation_name given ]" << std::endl;
+        return BT::NodeStatus::FAILURE;
+    }
 
     auto send_goal_options = Client<Movement>::SendGoalOptions();
     send_goal_options.feedback_callback = [&](__attribute__((unused)) std::shared_ptr<ClientGoalHandle<Movement>> goal_handle, std::shared_ptr<const Movement::Feedback> feedback) {

src/btree/src/nodes/OffsetPose.cpp

@@ -47,4 +47,3 @@ BT::NodeStatus BT::OffsetPose::tick() {
 
     return BT::NodeStatus::SUCCESS;
 }
-

src/btree/src/nodes/RandomFailure.cpp

@@ -6,7 +6,7 @@
 
 BT::NodeStatus BT::RandomFailure::tick() {
     double failureChance;
-    if (!getInput<double>("failureChance", failureChance)) {
+    if (!getInput<double>("failure_chance", failureChance)) {
         std::cout << "could not get failure chance." << std::endl;
         return NodeStatus::FAILURE;
     }
@@ -19,7 +19,7 @@ BT::NodeStatus BT::RandomFailure::tick() {
 }
 
 BT::PortsList BT::RandomFailure::providedPorts() {
-    return {InputPort<double>("failureChance", "Chance to fail from 0 to 1")};
+    return {InputPort<double>("failure_chance", "Chance to fail from 0 to 1")};
 }
 
 BT::RandomFailure::RandomFailure(

src/btree/trees/actorTreeCoex.xml

@@ -7,13 +7,13 @@
                     <Action ID="GenerateXYPose" area="{safeArea}" pose="{actorTarget}"/>
                     <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
                 </Control>
-                <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
-                <Action ID="ActorAnimation" animation="standing_extend_arm"/>
+                <Action ID="ActorAnimation" animation_name="standing_extend_arm"/>
-                <Action ID="ActorAnimation" animation="standing_retract_arm"/>
+                <Action ID="ActorAnimation" animation_name="standing_retract_arm"/>
             </Sequence>
             <Sequence>
                 <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
             </Sequence>
         </Control>
     </BehaviorTree>

src/btree/trees/actorTreeColab.xml

@@ -14,12 +14,12 @@
                         <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
                         <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
                     </Control>
-                    <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
+                    <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
                 </Sequence>
             </ReactiveSequence>
             <Sequence>
                 <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
                 <Action ID="SetCalledTo" state="false"/>
             </Sequence>
         </Fallback>

src/btree/trees/actorTreeCoop.xml

@@ -14,12 +14,12 @@
                         <Action ID="GenerateXYPose" area="{warningArea}" pose="{actorTarget}"/>
                         <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
                     </Control>
-                    <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
+                    <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
                 </Sequence>
             </ReactiveSequence>
             <Sequence>
                 <Action ID="GenerateXYPose" area="{unsafeArea}" pose="{actorTarget}"/>
-                <Action ID="ActorMovement" animation="walk" target="{actorTarget}"/>
+                <Action ID="ActorMovement" animation_name="walk" target="{actorTarget}"/>
                 <Action ID="SetCalledTo" state="false"/>
             </Sequence>
         </Fallback>

src/btree/trees/robotTreeColab.xml

@@ -16,11 +16,11 @@
                         <Action ID="GenerateXYPose" area="{negativeYTable}" pose="{target}"/>
                         <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
                         <Action ID="RobotMove" target="{target}"/>
-                        <Action ID="RandomFailure" failureChance="0.15"/>
+                        <Action ID="RandomFailure" failure_chance="0.15"/>
                         <Action ID="GenerateXYPose" area="{positiveYTable}" pose="{target}"/>
                         <Action ID="OffsetPose" input="{target}" offset="0,0,1.1" orientation="0,0,1,0" output="{target}"/>
                         <Action ID="RobotMove" target="{target}"/>
-                        <Action ID="RandomFailure" failureChance="0.05"/>
+                        <Action ID="RandomFailure" failure_chance="0.05"/>
                 </Control>
                 <Action ID="SetCalledTo" state="true"/>
             </Fallback>

src/btree_nodes/src/nodes/AmICalled.cpp

@@ -1,23 +0,0 @@
-//
-// Created by bastian on 26.03.22.
-//
-
-#include "AmICalled.h"
-
-BT::AmICalled::AmICalled(const std::string &name, const BT::NodeConfiguration &config) :
-        ConditionNode(name, config) {}
-
-BT::PortsList BT::AmICalled::providedPorts() {
-    return {InputPort<bool>("called")};
-}
-
-BT::NodeStatus BT::AmICalled::tick() {
-    bool called;
-    if (!getInput("called", called)) {
-        return NodeStatus::FAILURE;
-    }
-    if (called) {
-        return NodeStatus::SUCCESS;
-    }
-    return NodeStatus::FAILURE;
-}

src/btree_nodes/src/nodes/AmICalled.h

@@ -1,22 +0,0 @@
-//
-// Created by bastian on 26.03.22.
-//
-
-#ifndef BUILD_AMICALLED_H
-#define BUILD_AMICALLED_H
-
-#include <behaviortree_cpp_v3/condition_node.h>
-
-namespace BT {
-
-    class AmICalled : public ConditionNode {
-    public:
-        AmICalled(const std::string &name, const NodeConfiguration &config);
-
-        static PortsList providedPorts();
-
-        NodeStatus tick() override;
-    };
-
-}
-#endif //BUILD_AMICALLED_H

src/btree_trees/package.xml

@@ -1,13 +0,0 @@
-<package format="3">
-  <name>btree_trees</name>
-  <version>0.244.1</version>
-  <description>Trees for the simulation</description>
-  <license>Apache 2.0</license>
-  <maintainer email="bastian.hofmann@xitaso.com">Bastian Hofmann</maintainer>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/controller_test/CMakeLists.txt

@@ -1,17 +1,28 @@
 cmake_minimum_required(VERSION 3.8)
-project(btree_trees)
+project(controller_test)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(control_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-install(DIRECTORY   trees     DESTINATION   share/${PROJECT_NAME})
+add_executable(move src/test.cpp)
+set_property(TARGET move PROPERTY CXX_STANDARD 17)
+ament_target_dependencies(move rclcpp moveit_ros_planning_interface control_msgs)
+
+install(TARGETS
+        move
+        DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

src/controller_test/package.xml

@@ -0,0 +1,23 @@
+<?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>controller_test</name>
+  <version>0.1.0</version>
+  <description>
+     Test for Moveit on IISY
+  </description>
+  <author email="bastian.hofmann@xitaso.com">bastian</author>
+  <maintainer email="bastian.hofmann@xitaso.com">bastian</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

src/controller_test/src/test.cpp

@@ -0,0 +1,203 @@
+#include <control_msgs/action/detail/follow_joint_trajectory__struct.hpp>
+#include <memory>
+#include <rclcpp_action/client_goal_handle.hpp>
+#include <rclcpp_action/types.hpp>
+#include <string>
+#include <vector>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+
+std::shared_ptr<rclcpp::Node> node;
+
+void common_goal_response(
+  const rclcpp_action::ClientGoalHandle
+  <control_msgs::action::FollowJointTrajectory>::SharedPtr& future)
+{
+  RCLCPP_DEBUG(
+    node->get_logger(), "common_goal_response time: %f",
+    rclcpp::Clock().now().seconds());
+  auto goal_handle = future.get();
+  if (!goal_handle) {
+    printf("Goal rejected\n");
+  } else {
+    printf("Goal accepted\n");
+  }
+}
+
+void common_result_response(
+  const rclcpp_action::ClientGoalHandle
+  <control_msgs::action::FollowJointTrajectory>::WrappedResult & result)
+{
+  printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds());
+  switch (result.code) {
+    case rclcpp_action::ResultCode::SUCCEEDED:
+      printf("SUCCEEDED result code\n");
+      break;
+    case rclcpp_action::ResultCode::ABORTED:
+      printf("Goal was aborted\n");
+      return;
+    case rclcpp_action::ResultCode::CANCELED:
+      printf("Goal was canceled\n");
+      return;
+    default:
+      printf("Unknown result code\n");
+      return;
+  }
+}
+
+void common_feedback(
+  const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&,
+  const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback>& feedback)
+{
+  std::cout << "feedback->desired.positions :";
+  for (auto & x : feedback->desired.positions) {
+    std::cout << x << "\t";
+  }
+  std::cout << std::endl;
+  std::cout << "feedback->desired.velocities :";
+  for (auto & x : feedback->desired.velocities) {
+    std::cout << x << "\t";
+  }
+  std::cout << std::endl;
+}
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+
+  node = std::make_shared<rclcpp::Node>("trajectory_test_node");
+
+  std::cout << "node created" << std::endl;
+
+  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr action_client;
+  action_client = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
+    node->get_node_base_interface(),
+    node->get_node_graph_interface(),
+    node->get_node_logging_interface(),
+    node->get_node_waitables_interface(),
+    "/arm_controller/follow_joint_trajectory");
+
+  bool response =
+    action_client->wait_for_action_server(std::chrono::seconds(10));
+  if (!response) {
+    throw std::runtime_error("could not get action server");
+  }
+  std::cout << "Created action server" << std::endl;
+
+  std::vector<std::string> joint_names = {
+    "base_rot",
+    "base_link1_joint",
+    "link1_link2_joint",
+    "link2_rot",
+    "link2_link3_joint",
+    "link3_rot"
+    };
+
+  std::vector<trajectory_msgs::msg::JointTrajectoryPoint> points;
+  trajectory_msgs::msg::JointTrajectoryPoint point;
+  point.time_from_start = rclcpp::Duration::from_seconds(0.0);  // start asap
+  point.positions.resize(joint_names.size());
+
+  point.positions[0] = 0.0;
+  point.positions[1] = 0.0;
+  point.positions[2] = 0.0;
+  point.positions[3] = 0.0;
+  point.positions[4] = 0.0;
+  point.positions[5] = 0.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point2;
+  point2.time_from_start = rclcpp::Duration::from_seconds(1.0);
+  point2.positions.resize(joint_names.size());
+
+  point2.positions[0] = -1.0;
+  point2.positions[1] = 0.0;
+  point2.positions[2] = 0.0;
+  point2.positions[3] = 0.0;
+  point2.positions[4] = 0.0;
+  point2.positions[5] = 0.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point3;
+  point3.time_from_start = rclcpp::Duration::from_seconds(2.0);
+  point3.positions.resize(joint_names.size());
+
+  point3.positions[0] = 1.0;
+  point3.positions[1] = 0.0;
+  point3.positions[2] = 0.0;
+  point3.positions[3] = 0.0;
+  point3.positions[4] = 0.0;
+  point3.positions[5] = 0.0;
+
+  trajectory_msgs::msg::JointTrajectoryPoint point4;
+  point4.time_from_start = rclcpp::Duration::from_seconds(3.0);
+  point4.positions.resize(joint_names.size());
+
+  point4.positions[0] = 0.0;
+  point4.positions[1] = 0.0;
+  point4.positions[2] = M_PI/2;
+  point4.positions[3] = 0.0;
+  point4.positions[4] = 0.0;
+  point4.positions[5] = 0.0;
+
+  points.push_back(point);
+  points.push_back(point2);
+  points.push_back(point3);
+  points.push_back(point4);
+
+  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions opt;
+  opt.goal_response_callback = [](std::shared_ptr<rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>> goal){ 
+    if(goal->get_status()==rclcpp_action::GoalStatus::STATUS_ACCEPTED){
+      RCLCPP_DEBUG(
+        node->get_logger(), "common_goal_response accepted: %f",
+        rclcpp::Clock().now().seconds());
+    }else{
+      RCLCPP_DEBUG(
+        node->get_logger(), "common_goal_response rejected: %f",
+        rclcpp::Clock().now().seconds());
+    }
+  };
+  //opt.goal_response_callback = [](auto && PH1) { return common_goal_response(std::forward<decltype(PH1)>(PH1)); };
+  opt.result_callback = [](auto && PH1) { return common_result_response(std::forward<decltype(PH1)>(PH1)); };
+  opt.feedback_callback = [](auto && PH1, auto && PH2) { return common_feedback(std::forward<decltype(PH1)>(PH1), std::forward<decltype(PH2)>(PH2)); };
+
+  control_msgs::action::FollowJointTrajectory_Goal goal_msg;
+  goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0);
+  goal_msg.trajectory.joint_names = joint_names;
+  goal_msg.trajectory.points = points;
+
+  auto goal_handle_future = action_client->async_send_goal(goal_msg, opt);
+
+  if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
+    return 1;
+  }
+  RCLCPP_ERROR(node->get_logger(), "send goal call ok :)");
+
+  const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr&
+    goal_handle = goal_handle_future.get();
+  if (!goal_handle) {
+    RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
+    return 1;
+  }
+  RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server");
+
+  // Wait for the server to be done with the goal
+  auto result_future = action_client->async_get_result(goal_handle);
+  RCLCPP_INFO(node->get_logger(), "Waiting for result");
+  if (rclcpp::spin_until_future_complete(node, result_future) !=
+    rclcpp::FutureReturnCode::SUCCESS)
+  {
+    RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
+    return 1;
+  }
+
+  std::cout << "async_send_goal" << std::endl;
+  
+  rclcpp::shutdown();
+
+  return 0;
+}

src/ign_actor_plugin/src/ActorSystem.cpp

@@ -125,10 +125,11 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
         auto newTime = animationTimeComponent->Data() + _info.dt;
         auto newTimeSeconds = std::chrono::duration<double>(newTime).count();
         
-        feedback.progress = newTimeSeconds / duration;
+        auto progress = newTimeSeconds / (duration/animation_speed);
+        feedback.progress = progress;
         sendFeedback();
 
-        if (newTimeSeconds >= duration) {
+        if (progress >= 1.0) {
             igndbg << "Animation " << animation_name << " finished." << std::endl;
             nextState = IDLE;
             feedback.progress = 0.0f;
@@ -166,7 +167,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
             if(movementDetails.targetDiff.Pos().Length() >= 0.001){
                 movementDetails.targetDiff.Rot() = ignition::math::Quaterniond::EulerToQuaternion(0.0, 0.0, atan2(movementDetails.targetDiff.Pos().Y(), movementDetails.targetDiff.Pos().X()));
                 movementDetails.rotateToTargetDuration = Angle(actorPose.Rot(),movementDetails.targetDiff.Rot()) / turnSpeed;
-                movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_distance * duration;
+                movementDetails.moveDuration = movementDetails.targetDiff.Pos().Length() / animation_speed * duration;
             }else{
                 movementDetails.targetDiff.Rot() = movementDetails.start.Rot();
                 movementDetails.rotateToTargetDuration = 0.0;
@@ -190,6 +191,7 @@ void ActorSystem::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ec
 
         if (movementDetails.state == movementDetails.ROTATE_TO_TARGET){
             if(movementDetails.stepTime<=movementDetails.rotateToTargetDuration){
+                newTime = newTime.zero();
                 actorPose.Rot() = ignition::math::Quaterniond::Slerp(movementDetails.stepTime/movementDetails.rotateToTargetDuration,movementDetails.start.Rot(),movementDetails.targetDiff.Rot(),true);
             }else{
                 actorPose.Rot() = movementDetails.targetDiff.Rot();
@@ -311,8 +313,7 @@ void ActorSystem::messageQueueInterface(const char name[256]) {
             threadMutex.lock();
 
             strcpy(animation_name, receivedAction.animationName);
-            // animation_speed = receivedAction.animationSpeed;
+            animation_speed = receivedAction.animationSpeed;
-            animation_distance = receivedAction.animationDistance;
             movementDetails.target.Pos().Set(receivedAction.target.position.x, receivedAction.target.position.y, receivedAction.target.position.z);
             movementDetails.target.Rot().Set(receivedAction.target.orientation.w, receivedAction.target.orientation.x, receivedAction.target.orientation.y, receivedAction.target.orientation.z);
             nextState = receivedAction.state;

src/ign_actor_plugin/src/ActorSystem.hpp

@@ -59,7 +59,7 @@ class ActorSystem : public System,
     private:
         ignition::math::Pose3d startPose = ignition::math::Pose3d::Zero;
         ignition::math::Pose3d target_pose;
-        double animation_distance;
+        double animation_speed;
         char animation_name[256];
         ActorPluginState nextState,currentState,lastState;
         double duration{};

src/ign_world/launch/gazebo_controller_launch.py

@@ -32,6 +32,52 @@ def generate_launch_description():
         output='screen'
     )
     
+    btree = Node(
+        package='btree',
+        executable='tree',
+        output='both',
+        parameters=[
+            {
+                "trees": [
+                    get_package_share_directory('btree')+'/trees/actorTreeCoex.xml',
+                    get_package_share_directory('btree')+'/trees/robotTreeCoex.xml'
+                ],
+                "areas": [
+                    "safeArea",
+                    "1,   3.5 |" +
+                    "1,   7   |" +
+                    "3,   3.5 |" +
+                    "7,   7   |" +
+                    "3,  -1   |" +
+                    "7,  -1",
+                    "warningArea",
+                    "-1,  2.5 |" +
+                    "-1,  3.5 |" +
+                    "2,   2.5 |" +
+                    "3,   3.5 |" +
+                    "2,  -1   |" +
+                    "3,  -1",
+                    "unsafeArea",
+                    "-1,  1.5 |" +
+                    "-1,  2.5 |" +
+                    "1,   1.5 |" +
+                    "2,   2.5 |" +
+                    "1,  -1   |" +
+                    "2,  -1",
+                    "negativeYTable",
+                    "0.3,  -0.25 |" +
+                    "-0.3, -0.25 |" +
+                    "0,    -0.4",
+                    "positiveYTable",
+                    "0.3,   0.25 |" +
+                    "-0.3,  0.25 |" +
+                    "0,     0.4"
+                ]
+            },
+        ],
+        emulate_tty=True,
+    )
+
     return LaunchDescription([
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')),
@@ -66,51 +112,7 @@ def generate_launch_description():
             emulate_tty=True,
         ),
         
-        Node(
+        btree,
-            package='btree_nodes',
-            executable='tree',
-            output='both',
-            parameters=[
-                {
-                    "trees": [
-                        get_package_share_directory('btree_trees')+'/trees/actorTreeCoex.xml',
-                        get_package_share_directory('btree_trees')+'/trees/robotTreeCoex.xml'
-                    ],
-                    "areas": [
-                        "safeArea",
-                        "1,   3.5 |" +
-                        "1,   7   |" +
-                        "3,   3.5 |" +
-                        "7,   7   |" +
-                        "3,  -1   |" +
-                        "7,  -1",
-                        "warningArea",
-                        "-1,  2.5 |" +
-                        "-1,  3.5 |" +
-                        "2,   2.5 |" +
-                        "3,   3.5 |" +
-                        "2,  -1   |" +
-                        "3,  -1",
-                        "unsafeArea",
-                        "-1,  1.5 |" +
-                        "-1,  2.5 |" +
-                        "1,   1.5 |" +
-                        "2,   2.5 |" +
-                        "1,  -1   |" +
-                        "2,  -1",
-                        "negativeYTable",
-                        "0.3,  -0.25 |" +
-                        "-0.3, -0.25 |" +
-                        "0,    -0.4",
-                        "positiveYTable",
-                        "0.3,   0.25 |" +
-                        "-0.3,  0.25 |" +
-                        "0,     0.4"
-                    ]
-                },
-            ],
-            emulate_tty=True,
-        ),
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(

src/iisy_config/config/iisy.urdf

@@ -27,7 +27,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="-0.043517 0.000000 0.054914"/>
-      <mass value="0.1"/>
+      <mass value="6"/>
       <inertia ixx="1.277905" ixy="0.0" ixz="0.138279" iyy="3.070705" iyz="0.0" izz="3.284356"/>
     </inertial>
   </link>
@@ -45,7 +45,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.005 0.08"/>
-      <mass value="0.1"/>
+      <mass value="2"/>
       <inertia ixx="0.32666666666667" ixy="0.0" ixz="0.0" iyy="0.32666666666667" iyz="0.0" izz="0.25"/>
     </inertial>
   </link>
@@ -62,7 +62,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.050000 0.150000"/>
-      <mass value="0.1"/>
+      <mass value="4"/>
       <inertia ixx="8.266347" ixy="0.0" ixz="0.0" iyy="8.647519" iyz="0.0" izz="8.647519"/>
     </inertial>
   </link>
@@ -80,7 +80,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0.07 0.025"/>
-      <mass value="0.1"/>
+      <mass value="2"/>
       <inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.25"/>
     </inertial>
   </link>
@@ -97,7 +97,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0.037363 0.086674"/>
-      <mass value="0.1"/>
+      <mass value="2"/>
       <inertia ixx="1.431738" ixy="0.0" ixz="0.0" iyy="1.131426" iyz="-0.342192" izz="0.807202"/>
     </inertial>
   </link>
@@ -114,7 +114,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.055000 0.021413"/>
-      <mass value="0.1"/>
+      <mass value="2"/>
       <inertia ixx="0.362124" ixy="0.0" ixz="0.0" iyy="0.343531" iyz="0.0" izz="0.283368"/>
     </inertial>
   </link>
@@ -131,7 +131,7 @@
     </visual>
     <inertial>
       <origin rpy="0 0 0" xyz="0 -0.015462 0.040000"/>
-      <mass value="0.1"/>
+      <mass value="0.8"/>
       <inertia ixx="0.228470" ixy="0.0" ixz="0.0" iyy="0.137641" iyz="0.0" izz="0.252003"/>
     </inertial>
   </link>
@@ -155,31 +155,31 @@
     <parent link="base_bottom"/>
     <child link="base_top"/>
     <axis xyz="0 0 1"/>
-    <limit effort="30" velocity="1.7453292519943"/>
+    <limit effort="300" velocity="3.49065850399"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <dynamics damping="30.0" friction="0.1"/>
   </joint>
   <joint name="base_link1_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 -0.080499 0.092997"/>
     <parent link="base_top"/>
     <child link="link1_full"/>
     <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-2.2689280275926" upper="2.4434609527921" velocity="1.7453292519943"/>
+    <limit effort="300" lower="-2.2689280275926" upper="2.4434609527921" velocity="3.49065850399"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <dynamics damping="30.0" friction="0.1"/>
   </joint>
   <joint name="link1_link2_joint" type="revolute">
     <origin rpy="0 0 0" xyz="0 0 0.3"/>
     <parent link="link1_full"/>
     <child link="link2_bottom"/>
     <axis xyz="0 1 0"/>
-    <limit effort="30" lower="-2.6179938779915" upper="2.6179938779915" velocity="1.7453292519943"/>
+    <limit effort="200" lower="-2.6179938779915" upper="2.6179938779915" velocity="3.49065850399"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <dynamics damping="20.0" friction="0.1"/>
   </joint>
   <joint name="link2_rot" type="continuous">
     <origin rpy="0 0 0" xyz="0 0.080501 0.120502"/>
     <parent link="link2_bottom"/>
     <child link="link2_top"/>
     <axis xyz="0 0 1"/>
-    <limit effort="30" velocity="1.7453292519943"/>
+    <limit effort="100" velocity="5.23598775598"/>
     <dynamics damping="10.0" friction="0.1"/>
   </joint>
   <joint name="link2_link3_joint" type="continuous">
@@ -187,16 +187,16 @@
     <parent link="link2_top"/>
     <child link="link3_bottom"/>
     <axis xyz="0 1 0"/>
-    <limit effort="30" velocity="1.7453292519943"/>
+    <limit effort="50" velocity="5.23598775598"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <dynamics damping="5.0" friction="0.1"/>
   </joint>
   <joint name="link3_rot" type="continuous">
     <origin rpy="0 0 0" xyz="0 -0.055001 0.085501"/>
     <parent link="link3_bottom"/>
     <child link="link3_top"/>
     <axis xyz="0 0 1"/>
-    <limit effort="30" velocity="1.7453292519943"/>
+    <limit effort="50" velocity="6.98131700798"/>
-    <dynamics damping="10.0" friction="0.1"/>
+    <dynamics damping="5.0" friction="0.1"/>
   </joint>
   <transmission name="trans_base_rot">
     <type>transmission_interface/SimpleTransmission</type>

src/ros_actor_action_server/src/Server.cpp

@@ -122,6 +122,7 @@ int main(int argc, char **argv) {
             if (currentFeedback.state == IDLE) {
                 std::cout << "Accepting..." << std::endl;
                 
+                currentAction.animationSpeed = animationGoal->animation_speed;
                 strcpy(currentAction.animationName, animationGoal->animation_name.data());
 
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
@@ -163,7 +164,7 @@ int main(int argc, char **argv) {
                 std::cout << "Accepting..." << std::endl;
                 
                 currentAction.target = movementGoal->target;
-                currentAction.animationDistance = movementGoal->animation_distance;
+                currentAction.animationSpeed = movementGoal->animation_distance;
                 strcpy(currentAction.animationName, movementGoal->animation_name.data());
 
                 return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;

src/ros_actor_action_server_msgs/action/Movement.action

@@ -1,5 +1,4 @@
 string animation_name
-float32 animation_speed
 float32 animation_distance
 geometry_msgs/Pose target
 ---

src/ros_actor_message_queue_msgs/include/ros_actor_message_queue_msgs/MessageTypes.hpp

@@ -1,19 +1,21 @@
 #include <geometry_msgs/msg/detail/pose__struct.hpp>
 namespace ros_actor_message_queue_msgs{
+  
 enum ActorPluginState{
-  SETUP,IDLE,ANIMATION,MOVEMENT
+    SETUP,IDLE,ANIMATION,MOVEMENT
-  };
+};
 
 struct FeedbackMessage{
-  ActorPluginState state;
+    ActorPluginState state;
-  float progress;
+    float progress;
-  geometry_msgs::msg::Pose current;
+    geometry_msgs::msg::Pose current;
 };
 
 struct ActionMessage{
-  ActorPluginState state;
+    ActorPluginState state;
-  geometry_msgs::msg::Pose target;
+    geometry_msgs::msg::Pose target;
-  float animationSpeed = 1.0f,animationDistance = 1.0f;
+    float animationSpeed = 1.0f;
-  char animationName[256];
+    char animationName[256];
 };
+
 }