Action Server Fix (Incomplete)

robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml

@@ -4,7 +4,7 @@
     <BehaviorTree ID="BehaviorTree">
         <Sequence>
             <Action ID="GetEntityState" server_name="/get_entity_state" server_timeout="10" PartName="rasmt"/>
-            <Action ID="MoveToPose" server_name="/move_topose" server_timeout="10" cancel_timeout="5" arm_group="${arg0}" />
+            <Action ID="MoveToPose" server_name="/move_topose" server_timeout="10" cancel_timeout="5" arm_group="rasmt_arm_group" />
         </Sequence>
     </BehaviorTree>
     <!-- ////////// -->

robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp

@@ -1,6 +1,7 @@
 #include "robossembler_interfaces/action/moveit_send_pose.hpp"
 #include "behaviortree_cpp_v3/bt_factory.h"
 #include "behavior_tree/BtAction.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 using namespace BT;
 using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
@@ -21,12 +22,14 @@ class MoveToPose : public BtAction<MoveToPoseAction>
 
         MoveToPoseAction::Goal populate_goal() override
         {
-            MoveToPoseAction::Goal goal;
+            //MoveToPoseAction::Goal goal;
+            auto goal = MoveToPoseAction::Goal();
             //auto goal = std::make_shared<MoveToPoseAction::Goal>();
             goal.robot_name = robot_name_;
-            goal.end_effector_acceleration = 1;
-            goal.end_effector_velocity = 1;
+            goal.end_effector_acceleration = 1.0;
+            goal.end_effector_velocity = 1.0;
             goal.target_pose = target_pose_;
+            RCLCPP_INFO(_node->get_logger(), "Goal populated");
 
             return goal;
         }

robossembler/src/bt_checker.cpp

@@ -41,9 +41,11 @@ void BtEngine::run()
   RCLCPP_INFO_STREAM(
     this->get_logger(), "Running tree at frequency " <<
       1.0 / loop_timeout_.count() * 1e3 << "Hz");
+    while(rclcpp::ok()){
         tree_->tickRoot();
         loop_rate.sleep();
     }
+}
 
 void BtEngine::add_groot_monitoring()
 {

robossembler_interfaces/CMakeLists.txt

@@ -18,11 +18,13 @@ endif()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
     "action/MoveitSendPose.action"
     "msg/ActionFeedbackStatusConstants.msg"
     "msg/ActionResultStatusConstants.msg"
+    DEPENDENCIES geometry_msgs
 )
 
 if(BUILD_TESTING)

robossembler_servers/CMakeLists.txt

@@ -51,7 +51,7 @@ if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
 endif()
-#
+#[[
 add_library(move_topose_action SHARED 
     src/move_topose_action_server.cpp
 )
@@ -62,17 +62,15 @@ target_include_directories(move_topose_action PRIVATE
 ament_target_dependencies(move_topose_action ${deps})
 
 rclcpp_components_register_node(move_topose_action PLUGIN "robossembler_actions::MoveToPoseActionServer" EXECUTABLE move_topose_action_server)
-
-#[[
+]]
+#
 add_executable(move_topose_action_server src/move_topose_action_server.cpp)
 target_include_directories(move_topose_action_server PRIVATE include)
 ament_target_dependencies(move_topose_action_server ${deps})
-]]
+
 install(
   TARGETS move_topose_action_server
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
+  DESTINATION lib/${PROJECT_NAME}
 )
 
 ament_package()

robossembler_servers/src/move_topose_action_server.cpp

@@ -18,6 +18,7 @@
 // moveit libs
 #include "moveit/move_group_interface/move_group_interface.h"
 #include "moveit/planning_interface/planning_interface.h"
+
 /*
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -39,12 +40,14 @@ public:
     using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
 
     //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node) 
-    explicit MoveToPoseActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
-    : Node("move_topose_action_server", options)//, node_(node)
+    explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose"), node_(node)
     {
         using namespace std::placeholders;
         this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
-            this,
+            this->get_node_base_interface(),
+            this->get_node_clock_interface(),
+            this->get_node_logging_interface(),
+            this->get_node_waitables_interface(),
             "move_topose",
             std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
             std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
@@ -56,12 +59,13 @@ private:
     rclcpp_action::Server<MoveitSendPose>::SharedPtr action_server_;
 
     using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendPose>;
+
     rclcpp_action::GoalResponse goal_callback(
         const rclcpp_action::GoalUUID & uuid,
         std::shared_ptr<const MoveitSendPose::Goal> goal)
     {
         RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]", 
-            goal->robot_name, goal->target_pose.pose.position.x, goal->target_pose.pose.position.y, goal->target_pose.pose.position.z,
+            goal->robot_name.c_str(), goal->target_pose.pose.position.x, goal->target_pose.pose.position.y, goal->target_pose.pose.position.z,
             goal->target_pose.pose.orientation.x, goal->target_pose.pose.orientation.y, goal->target_pose.pose.orientation.z, 
             goal->target_pose.pose.orientation.w);
         (void)uuid;
@@ -89,15 +93,18 @@ private:
         RCLCPP_INFO(this->get_logger(), "Executing goal");
         const auto goal = goal_handle->get_goal();
         auto result = std::make_shared<MoveitSendPose::Result>();
-
+        RCLCPP_INFO(this->get_logger(), "Goal and Result Initialised");
         moveit::planning_interface::MoveGroupInterface::Options move_group_options(goal->robot_name, "robot_description");
+        RCLCPP_INFO(this->get_logger(), "move_group_options");
         moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
         moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
-
+        RCLCPP_INFO(this->get_logger(), "Moveit2 Initialised");
         move_group_interface.setStartState(start_state);
-        move_group_interface.setPoseTarget(goal->target_pose.pose);
+        //move_group_interface.setStartStateToCurrentState();
+        move_group_interface.setPoseTarget(goal->target_pose);
         move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
         move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
+        RCLCPP_INFO(this->get_logger(), "move_group ready to plan");
 
         moveit::planning_interface::MoveGroupInterface::Plan plan;
         bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
@@ -124,16 +131,19 @@ private:
 
 }// namespace robossembler_actions
 
-RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_actions::MoveToPoseActionServer)
-
-/*
-int main(int argc, char * argv[])
+int main(int argc, char ** argv)
 {
     rclcpp::init(argc, argv);
     rclcpp::NodeOptions node_options;
     node_options.automatically_declare_parameters_from_overrides(true);
-    rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("move_topose", "", node_options);
-    rclcpp::spin(node);
-    //rclcpp::shutdown();
+    auto node = rclcpp::Node::make_shared("move_topose", node_options);
+    auto action_server = std::make_shared<robossembler_actions::MoveToPoseActionServer>(node);
+
+    //rclcpp::spin(action_server);
+
+    rclcpp::executors::MultiThreadedExecutor executor;
+    executor.add_node(action_server);
+    executor.spin();
+
     return 0;
-}*/
+}