diff --git a/rasmt_moveit_config/launch/rasmt_moveit.launch.py b/rasmt_moveit_config/launch/rasmt_moveit.launch.py
index 6ad8756..9d66f48 100644
--- a/rasmt_moveit_config/launch/rasmt_moveit.launch.py
+++ b/rasmt_moveit_config/launch/rasmt_moveit.launch.py
@@ -159,7 +159,25 @@ def generate_launch_description():
move_topose_action_server = Node(
package="robossembler_servers",
executable="move_topose_action_server",
- name="m2p_as",
+ name="move_to_pose_moveit",
+ parameters=[
+ robot_description,
+ robot_description_semantic,
+ kinematics_yaml,
+ robot_description_planning,
+ ompl_yaml,
+ planning,
+ trajectory_execution,
+ moveit_controllers,
+ planning_scene_monitor_parameters,
+ use_sim_time
+ ]
+ )
+
+ move_to_joint_state_action_server = Node(
+ package="robossembler_servers",
+ executable="move_to_joint_states_action_server",
+ name="joint_states_moveit",
parameters=[
robot_description,
robot_description_semantic,
@@ -178,6 +196,7 @@ def generate_launch_description():
launch_nodes.append(rviz)
launch_nodes.append(move_group_node)
launch_nodes.append(move_topose_action_server)
+ launch_nodes.append(move_to_joint_state_action_server)
diff --git a/robossembler/CMakeLists.txt b/robossembler/CMakeLists.txt
index b48e01b..ac9307c 100644
--- a/robossembler/CMakeLists.txt
+++ b/robossembler/CMakeLists.txt
@@ -50,20 +50,14 @@ set(dependencies
include_directories(include)
-add_library(robossembler_move_topose_bt_action SHARED src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp)
-list(APPEND plugin_libs robossembler_move_topose_bt_action)
+add_library(robossembler_move_topose_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp)
+list(APPEND plugin_libs robossembler_move_topose_bt_action_client)
add_library(robossembler_get_entity_state_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp)
list(APPEND plugin_libs robossembler_get_entity_state_bt_action_client)
-add_library(robossembler_emu_pose_estimation SHARED src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp)
-list(APPEND plugin_libs robossembler_move_bt_node)
-
-add_library(robossembler_move_bt_node SHARED src/behavior_tree_nodes/atomic_skills/Move.cpp)
-list(APPEND plugin_libs robossembler_emu_pose_estimation)
-
-add_library(robossembler_move_gripper_bt_node SHARED src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp)
-list(APPEND plugin_libs robossembler_move_gripper_bt_node)
+add_library(robossembler_move_gripper_bt_action_client SHARED src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp)
+list(APPEND plugin_libs robossembler_move_gripper_bt_action_client)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
@@ -73,9 +67,6 @@ endforeach()
add_executable(move_controller_node src/move_controller_node.cpp)
ament_target_dependencies(move_controller_node ${dependencies})
-add_executable(bt_check src/bt_checker.cpp)
-ament_target_dependencies(bt_check ${dependencies})
-
install(DIRECTORY launch pddl behavior_trees_xml config DESTINATION share/${PROJECT_NAME})
@@ -87,10 +78,6 @@ install(TARGETS
RUNTIME DESTINATION bin
)
-install(TARGETS bt_check
- DESTINATION lib/${PROJECT_NAME}
- )
-
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
diff --git a/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml b/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml
index ec0981d..c3cd347 100644
--- a/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml
+++ b/robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml
@@ -3,8 +3,9 @@
-
-
+
+
+
diff --git a/robossembler/config/params.yaml b/robossembler/config/params.yaml
index c467a22..ba26926 100644
--- a/robossembler/config/params.yaml
+++ b/robossembler/config/params.yaml
@@ -1,17 +1,6 @@
assemble_1:
ros__parameters:
plugins:
- - robossembler_move_topose_bt_action
- robossembler_get_entity_state_bt_action_client
- PartName: "rasmt"
- arm_group: ["rasmt_arm_group"]
- gripper_group: ["rasmt_hand_arm_group"]
- relative_gaps: ["open", "close"]
- gaps:
- open: 0.039
- close: 0.0
- waypoints: ["one"]
- waypoint_coords:
- one: [0.01, 0.01, 0.6,
- 0.0 , 0.0 , 0.0,
- 1.0]
+ - robossembler_move_topose_bt_action_client
+ - robossembler_move_gripper_bt_action_client
diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp
deleted file mode 100644
index ea9ae97..0000000
--- a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp
+++ /dev/null
@@ -1,41 +0,0 @@
-#pragma once
-
-#include
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-#include "geometry_msgs/msg/pose_stamped.hpp"
-#include
-#include "plansys2_bt_actions/BTActionNode.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-
-namespace task_planner
-{
- class EmuPoseEstimation : public BT::ActionNodeBase
- {
- public:
- EmuPoseEstimation(const std::string &xml_tag, const BT::NodeConfiguration &conf);
-
- void resultCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
-
- /**
- * @brief force end of action
- */
- virtual void halt() override;
- BT::NodeStatus tick() override;
- /**
- * @brief provides the node with the user
- * arguments declared in the action.xml
- * @return BT::PortsList
- */
- static BT::PortsList providedPorts()
- {
- return {BT::InputPort("PartName")};
- }
-
- private:
- std::vector available_model_name_;
- std::atomic_bool _halt_request;
- std::string model_name_;
- };
-}
\ No newline at end of file
diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp
deleted file mode 100644
index 6f9660a..0000000
--- a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp
+++ /dev/null
@@ -1,55 +0,0 @@
-#pragma once
-
-#include
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-#include "geometry_msgs/msg/pose.hpp"
-#include "moveit_msgs/msg/move_it_error_codes.hpp"
-//#include "plansys2_bt_actions/BTActionNode.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-namespace task_planner
-{
- class Move : public BT::ActionNodeBase
- {
- public:
- Move(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf);
-
- void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg);
- /**
- * @brief force end of action
- */
- virtual void halt();
-
- /**
- * @brief the main body of the node's action until
- * its completion with a return BT::NodeStatus::SUCCESS
- * @return BT::NodeStatus
- */
- BT::NodeStatus tick();
-
- /**
- * @brief provides the node with the user
- * arguments declared in the action.xml
- * @return BT::PortsList
- */
- static BT::PortsList providedPorts()
- {
- return {
- BT::InputPort("arm_group"),
- BT::InputPort("goal")
- };
- }
-
- private:
- std::map waypoints_;
- std::vector available_groups_;
- std::string arm_group_;
- std::atomic_bool _halt_requested;
- };
-
-} // namespace task_planner
\ No newline at end of file
diff --git a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp b/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp
deleted file mode 100644
index 0f04425..0000000
--- a/robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp
+++ /dev/null
@@ -1,56 +0,0 @@
-#pragma once
-
-#include
-#include
-
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-#include "moveit_msgs/msg/move_it_error_codes.hpp"
-#include "plansys2_bt_actions/BTActionNode.hpp"
-#include "rclcpp/rclcpp.hpp"
-
-namespace task_planner
-{
- class MoveGripper : public BT::ActionNodeBase
- {
- public:
- MoveGripper(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf);
-
- void resultCallback(const moveit_msgs::msg::MoveItErrorCodes::SharedPtr msg);
- /**
- * @brief force end of action
- */
- virtual void halt();
-
- /**
- * @brief the main body of the node's action until
- * its completion with a return BT::NodeStatus::SUCCESS
- * @return BT::NodeStatus
- */
- BT::NodeStatus tick();
-
- /**
- * @brief provides the node with the user
- * arguments declared in the action.xml
- * @return BT::PortsList
- */
- static BT::PortsList providedPorts()
- {
- return {
- BT::InputPort("gripper_group"),
- BT::InputPort("relative_gap")
- };
- }
-
- private:
- const double max_gripper_gap_ = 0.04;
-
- std::map relative_gaps_;
- std::vector available_groups_;
- std::string gripper_group_;
- std::atomic_bool _halt_requested;
- };
-
-} // namespace task_planner
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp
deleted file mode 100644
index 87da47c..0000000
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-#include
-#include
-#include
-#include
-#include
-#include
-#include "robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp"
-
-
-namespace task_planner
-{
- static const rclcpp::Logger LOGGER = rclcpp::get_logger("PartPoseEstimation");
-
- EmuPoseEstimation::EmuPoseEstimation(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf)
- : ActionNodeBase(xml_tag_name, conf)
- {
- rclcpp::Node::SharedPtr node;
- config().blackboard->get("node", node);
- RCLCPP_INFO(LOGGER, "EmuPoseEstimation constructor started");
- try
- {
- node->declare_parameter("PartName");
- }
- catch(const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex)
- {
- RCLCPP_WARN(LOGGER, "%c", ex.what());
- }
- if (node->has_parameter("PartName"))
- {
- node->get_parameter_or("PartName", available_model_name_, {});
- }else
- {
- RCLCPP_WARN(LOGGER, "No model_name provided");
- }
- }
-
- void EmuPoseEstimation::halt()
- {
- _halt_request = true;
- RCLCPP_INFO(LOGGER,"EmuPoseEstimation halt");
- }
-
- BT::NodeStatus EmuPoseEstimation::tick()
- {
- rclcpp::Node::SharedPtr gz_get_entity_state;
- config().blackboard->get("node", gz_get_entity_state);
- RCLCPP_INFO(LOGGER, "EmuPoseEstimation action started");
-
- rclcpp::Client::SharedPtr client =
- gz_get_entity_state->create_client
- ("/get_entity_state");
-
- rclcpp::executors::SingleThreadedExecutor executor;
- executor.add_node(gz_get_entity_state);
- std::thread([&executor]() {executor.spin();}).detach();
-
- std::string gazebo_model_name;
- getInput("PartName",gazebo_model_name);
-
- if(std::find(available_model_name_.begin(), available_model_name_.end(), gazebo_model_name) != available_model_name_.end())
- {
- model_name_=gazebo_model_name;
- RCLCPP_INFO(LOGGER, "Provided model name [%s]", gazebo_model_name.c_str());
- }else{
- RCLCPP_WARN(LOGGER,"Provided not allowed model name [%s]", gazebo_model_name.c_str());
- }
-
- auto request = std::make_shared();
- request->name=model_name_;
-
- RCLCPP_INFO(LOGGER, "Provided service call with model name [%s]", request->name.c_str());
-
- while (!client->wait_for_service(std::chrono::seconds(1))) {
- if (!rclcpp::ok()) {
- RCLCPP_ERROR(LOGGER, "Interruped while waiting for the server.");
- return BT::NodeStatus::FAILURE;
- }
- RCLCPP_INFO(LOGGER, "Server not available, waiting again...");
- }
-
- auto result = client->async_send_request(request);
- RCLCPP_INFO(LOGGER, "Get Pose x = %f y = %f z = %f", result.get()->state.pose.position.x,
- result.get()->state.pose.position.y, result.get()->state.pose.position.z);
- return BT::NodeStatus::SUCCESS;
- }
-
-}
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("EmuPoseEstimation");
-}
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp
deleted file mode 100644
index 24e6bd2..0000000
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp
+++ /dev/null
@@ -1,179 +0,0 @@
-#include
-#include
-
-#include "rclcpp/executors/single_threaded_executor.hpp"
-//#include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "rclcpp/qos.hpp"
-
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-
-#include "moveit/move_group_interface/move_group_interface.h"
-#include "moveit/planning_scene_interface/planning_scene_interface.h"
-
-#include "robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp"
-
-namespace task_planner
-{
- static const rclcpp::Logger LOGGER = rclcpp::get_logger("task_planner_move_bt_node");
-
- Move::Move(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf)
- : ActionNodeBase(xml_tag_name, conf)
- {
- rclcpp::Node::SharedPtr node;
- config().blackboard->get("node", node);
- RCLCPP_INFO(LOGGER, "Move consructor started");
- try
- {
- node->declare_parameter("arm_group");
- node->declare_parameter("waypoints");
- node->declare_parameter("waypoint_coords");
- }
- catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex)
- {
- RCLCPP_WARN(LOGGER, "%c", ex.what());
- }
-
- if (node->has_parameter("arm_group"))
- {
- node->get_parameter_or("arm_group", available_groups_, {});
- }else{
-
- RCLCPP_WARN(LOGGER, "No arm_group names provided");
- }
-
- if (node->has_parameter("waypoints"))
- {
- std::vector wp_names;
- node->get_parameter_or("waypoints", wp_names, {});
-
- for (auto &wp : wp_names)
- {
- try
- {
- node->declare_parameter("waypoint_coords." + wp);
- }catch(const rclcpp::exceptions::ParameterAlreadyDeclaredException & e){
-
- }
-
- std::vector coords;
- if (node->get_parameter_or("waypoint_coords." + wp, coords, {}))
- {
- geometry_msgs::msg::Pose pose;
- pose.position.x = coords[0];
- pose.position.y = coords[1];
- pose.position.z = coords[2];
- if (coords.size() < 4)
- {
- pose.orientation.x = 0.0;
- pose.orientation.y = 0.0;
- pose.orientation.z = 0.0;
- pose.orientation.w = 1.0;
- } else {
- pose.orientation.x = coords[3];
- pose.orientation.y = coords[4];
- pose.orientation.z = coords[5];
- pose.orientation.w = coords[6];
- }
- waypoints_[wp] = pose;
- } else {
-
- RCLCPP_WARN(LOGGER, "No coordinate configured for waypoint [%s]", wp.c_str());
- }
- }
- }
- }
-
- void Move::halt()
- {
- _halt_requested = true;
- RCLCPP_INFO(LOGGER, "Move halt");
- }
-
- BT::NodeStatus Move::tick()
- {
- RCLCPP_INFO(LOGGER, "Move action started");
- rclcpp::NodeOptions node_options;
- node_options.automatically_declare_parameters_from_overrides(true);
- auto move_group_node = rclcpp::Node::make_shared("robossembler_move_group_interface", node_options);
-
- config().blackboard->get("node", move_group_node);
-
- rclcpp::executors::SingleThreadedExecutor executor;
- executor.add_node(move_group_node);
- std::thread([&executor]() { executor.spin(); }).detach();
-
- std::string goal;
- getInput("goal", goal);
-
- std::string arm_group;
- getInput("arm_group", arm_group);
-
- if (std::find(available_groups_.begin(), available_groups_.end(), arm_group) != available_groups_.end())
- {
- arm_group_ = arm_group;
- RCLCPP_INFO(LOGGER, "Provided available move_group [%s]", arm_group.c_str());
- }else{
- RCLCPP_WARN(LOGGER, "Provided not allowed move_group name [%s]", arm_group.c_str());
- }
-
- geometry_msgs::msg::Pose pose2moveit;
- if (waypoints_.find(goal) != waypoints_.end())
- {
- pose2moveit = waypoints_[goal];
- RCLCPP_INFO(LOGGER, "Read goal (%s) pose:[%f, %f, %f] orientation:[%f, %f, %f, %f]", goal.c_str(),
- pose2moveit.position.x, pose2moveit.position.y, pose2moveit.position.z,
- pose2moveit.orientation.x, pose2moveit.orientation.y, pose2moveit.orientation.z,
- pose2moveit.orientation.w);
- } else {
- RCLCPP_WARN(LOGGER, "No coordinate for waypoint [%s]", goal.c_str());
- }
- //Setup MoveIt interface for planning
- static const std::string PLANNING_GROUP = arm_group_;
- RCLCPP_INFO(LOGGER, "MoveGroupInterface set current planning group `%s`", PLANNING_GROUP.c_str());
-
- moveit::planning_interface::MoveGroupInterface::Options move_group_options(PLANNING_GROUP, "robot_description");
- moveit::planning_interface::MoveGroupInterface move_group_interface(move_group_node, move_group_options);
-
- moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
- move_group_interface.setStartState(start_state);
-
- geometry_msgs::msg::Pose goal_pos;
- goal_pos.orientation.x = pose2moveit.orientation.x;
- goal_pos.orientation.y = pose2moveit.orientation.y;
- goal_pos.orientation.z = pose2moveit.orientation.z;
- goal_pos.orientation.w = pose2moveit.orientation.w;
-
- goal_pos.position.x = pose2moveit.position.x;
- goal_pos.position.y = pose2moveit.position.y;
- goal_pos.position.z = pose2moveit.position.z;
-
- RCLCPP_INFO(LOGGER, "Move to goal pose:[%f, %f, %f] orientation:[%f, %f, %f, %f] has started",
- pose2moveit.position.x, pose2moveit.position.y, pose2moveit.position.z,
- pose2moveit.orientation.x, pose2moveit.orientation.y, pose2moveit.orientation.z,
- pose2moveit.orientation.w);
- move_group_interface.setPoseTarget(goal_pos);
-
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
- bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- if (success)
- {
- RCLCPP_INFO(LOGGER, "Planning success");
- move_group_interface.execute(my_plan);
- move_group_interface.move();
- } else {
-
- RCLCPP_WARN(LOGGER, "Failed to generate a plan");
- return BT::NodeStatus::FAILURE;
- }
-
- return BT::NodeStatus::SUCCESS;
- }
-
-} // namespace atomic_skills
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-BT_REGISTER_NODES(factory)
-{
- factory.registerNodeType("Move");
-}
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
index 0c40742..ab8646f 100644
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
+++ b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
@@ -1,154 +1,50 @@
-#include
-#include
-
-#include "rclcpp/executors/single_threaded_executor.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
-#include "rclcpp/qos.hpp"
-
-#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
-
-#include "moveit/move_group_interface/move_group_interface.h"
-#include "moveit/planning_scene_interface/planning_scene_interface.h"
-
-#include "robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp"
-
-namespace task_planner
-{
- static const rclcpp::Logger LOGGER = rclcpp::get_logger("task_planner_move_gripper_bt_node");
-
- MoveGripper::MoveGripper(const std::string &xml_tag_name,
- const BT::NodeConfiguration &conf)
- : ActionNodeBase(xml_tag_name, conf)
- {
- rclcpp::Node::SharedPtr node;
- config().blackboard->get("node", node);
- RCLCPP_INFO(LOGGER, "MoveGripper constructor started");
-
- try
- {
- node->declare_parameter("gripper_group");
- node->declare_parameter("relative_gaps");
- node->declare_parameter("gaps");
- }
- catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex)
- {
- RCLCPP_WARN(LOGGER, "%c", ex.what());
- }
-
- if (node->has_parameter("gripper_group"))
- {
- node->get_parameter_or("gripper_group", available_groups_, {});
- }else{
-
- RCLCPP_WARN(LOGGER, "No gripper_group names provided");
- }
-
- if (node->has_parameter("relative_gaps"))
- {
- std::vector gap_names;
- node->get_parameter_or("relative_gaps", gap_names, {});
- for (auto &gap: gap_names)
- {
- try
- {
- node->declare_parameter("gaps." + gap);
- }catch(const rclcpp::exceptions::ParameterAlreadyDeclaredException & e){
-
- }
-
- double gap_value;
- if (node->get_parameter_or("gaps." + gap, gap_value, {}))
- {
- relative_gaps_[gap] = gap_value;
- } else {
-
- RCLCPP_WARN(LOGGER, "No gap value configured for relative_gap [%s]", gap.c_str());
- }
-
- }
- }
- }
-
- void MoveGripper::halt()
- {
- _halt_requested = true;
- RCLCPP_INFO(LOGGER, "MoveGripper halt");
- }
-
- BT::NodeStatus MoveGripper::tick()
- {
- RCLCPP_INFO(LOGGER, "MoveGripper action started");
-
- rclcpp::NodeOptions node_options;
- node_options.automatically_declare_parameters_from_overrides(true);
- auto move_group_gripper_node = rclcpp::Node::make_shared("robossembler_move_group_interface", node_options);
-
- config().blackboard->get("node", move_group_gripper_node);
-
- rclcpp::executors::SingleThreadedExecutor executor;
- executor.add_node(move_group_gripper_node);
- std::thread([&executor]() { executor.spin(); }).detach();
-
- std::string gripper_group;
- getInput("gripper_group", gripper_group);
-
- std::string gap;
- getInput("relative_gap", gap);
-
- if (std::find(available_groups_.begin(), available_groups_.end(), gripper_group) != available_groups_.end())
- {
- gripper_group_ = gripper_group;
- RCLCPP_INFO(LOGGER, "Provided available move_group [%s]", gripper_group.c_str());
- }else{
- RCLCPP_WARN(LOGGER, "Provided not allowed move_group name [%s]", gripper_group.c_str());
- }
-
- double gap2moveit;
- if (relative_gaps_.find(gap) != relative_gaps_.end())
- {
- gap2moveit = relative_gaps_[gap];
- RCLCPP_INFO(LOGGER, "Read gap (%s) value:[%f]", gap, gap2moveit);
- } else {
-
- RCLCPP_WARN(LOGGER, "No value for relative_gap [%s]", gap.c_str());
- }
-
- static const std::string PLANNING_GROUP = gripper_group_;
- RCLCPP_INFO(LOGGER, "MoveGroupInterface set current planning group `%s`", PLANNING_GROUP.c_str());
-
- moveit::planning_interface::MoveGroupInterface::Options move_group_options(PLANNING_GROUP, "robot_description");
- moveit::planning_interface::MoveGroupInterface move_group_interface(move_group_gripper_node, move_group_options);
-
- auto gripper_joint_values = move_group_interface.getCurrentJointValues();
- moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
- move_group_interface.setStartState(start_state);
-
- for (auto &joint_value : gripper_joint_values) {
- joint_value = gap2moveit > 0? std::min(gap2moveit, max_gripper_gap_): 0.0;
- }
-
- move_group_interface.setJointValueTarget(gripper_joint_values);
-
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
- bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
- if (success)
- {
- RCLCPP_INFO(LOGGER, "Planning success");
- move_group_interface.execute(my_plan);
- move_group_interface.move();
- } else {
-
- RCLCPP_WARN(LOGGER, "Failed to generate a plan");
- return BT::NodeStatus::FAILURE;
- }
-
- return BT::NodeStatus::SUCCESS;
- }
-
-} // namespace atomic_skills
-
+#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behavior_tree/BtAction.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "moveit_msgs/action/move_group.h"
+
+using namespace BT;
+using MoveToJointStateAction = robossembler_interfaces::action::MoveitSendJointStates;
+static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_to_joitn_state_action_client");
+
+class MoveToJointState : public BtAction
+{
+ public:
+ MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
+ : BtAction(name, config) {
+
+ robot_name_ = getInput("arm_group").value();
+
+ gripper_gap = 0.02;
+ RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
+ }
+
+ MoveToJointStateAction::Goal populate_goal() override
+ {
+ auto goal = MoveToJointStateAction::Goal();
+ goal.robot_name = robot_name_;
+ goal.joints_acceleration_scaling_factor = 0.1;
+ goal.joints_velocity_scaling_factor = 0.1;
+ goal.joint_value = gripper_gap;
+ RCLCPP_INFO(_node->get_logger(), "Goal populated");
+
+ return goal;
+ }
+
+ static PortsList providedPorts()
+ {
+ return providedBasicPorts({
+ InputPort("arm_group")});
+ }
+
+ private:
+ double gripper_gap{0};
+ std::string robot_name_;
+
+}; // class MoveToJointState
+
BT_REGISTER_NODES(factory)
{
- factory.registerNodeType("MoveGripper");
-}
+ factory.registerNodeType("MoveGripper");
+}
\ No newline at end of file
diff --git a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
index 9a4bdec..440105e 100644
--- a/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
+++ b/robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
@@ -2,6 +2,7 @@
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "moveit_msgs/action/move_group.h"
using namespace BT;
using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
@@ -14,17 +15,15 @@ class MoveToPose : public BtAction
: BtAction(name, config) {
robot_name_ = getInput("arm_group").value();
- target_pose_.pose.position.x = 0.1;
- target_pose_.pose.position.y = 0.1;
- target_pose_.pose.position.z = 0.6;
+ target_pose_.position.x = 0.1;
+ target_pose_.position.y = 0.1;
+ target_pose_.position.z = 0.6;
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
}
MoveToPoseAction::Goal populate_goal() override
{
- //MoveToPoseAction::Goal goal;
auto goal = MoveToPoseAction::Goal();
- //auto goal = std::make_shared();
goal.robot_name = robot_name_;
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
@@ -41,8 +40,7 @@ class MoveToPose : public BtAction
}
private:
- //std::map waypoints_;
- geometry_msgs::msg::PoseStamped target_pose_;
+ geometry_msgs::msg::Pose target_pose_;
std::string robot_name_;
}; // class MoveToPose
diff --git a/robossembler/src/bt_checker.cpp b/robossembler/src/bt_checker.cpp
deleted file mode 100644
index d18852a..0000000
--- a/robossembler/src/bt_checker.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-
-#include "behavior_tree/BtEngine.hpp"
-#include
-
-BtEngine::BtEngine()
-: Node("bt_engine")
-{
- configure_parameters();
- load_plugins();
- load_tree();
- if (run_groot_monitoring_) {
- add_groot_monitoring();
- }
- run();
-}
-
-void BtEngine::configure_parameters()
-{
- bt_file_path_ = this->declare_parameter("bt_file_path", "simple_sequence.xml");
- loop_timeout_ = std::chrono::milliseconds(this->declare_parameter("loop_timeout", 100));
- plugins_ = this->declare_parameter("plugins", std::vector());
- // Groot
- run_groot_monitoring_ = this->declare_parameter("run_groot_monitoring", true);
- publisher_port_ = this->declare_parameter("publisher_port", 1666);
- server_port_ = this->declare_parameter("server_port", 1667);
- max_msg_per_second_ = this->declare_parameter("max_msg_per_second", 25);
-}
-
-void BtEngine::load_tree()
-{
- auto blackboard = Blackboard::create();
- blackboard->set("node", std::make_shared("bt_node"));
-// blackboard->set>("tf_buffer", tf_);
- RCLCPP_INFO(this->get_logger(), "Loading tree from file: " + bt_file_path_);
- tree_ = std::make_shared(factory_.createTreeFromFile(bt_file_path_, blackboard));
-}
-
-void BtEngine::run()
-{
- rclcpp::WallRate loop_rate(loop_timeout_);
- 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()
-{
- RCLCPP_INFO_STREAM(
- this->get_logger(), "Groot monitoring enabled with server port [" <<
- server_port_ << "] and publisher port [" << publisher_port_ << "]");
- groot_monitor_ = std::make_unique(
- *tree_, max_msg_per_second_, publisher_port_, server_port_);
-}
-
-void BtEngine::load_plugins()
-{
- for (const auto & p : plugins_) {
- RCLCPP_INFO(this->get_logger(), "Loading plugin: " + SharedLibrary::getOSName(p));
- factory_.registerFromPlugin(SharedLibrary::getOSName(p));
- }
-}
-
-void sigint_handler(__attribute__((unused)) int signal_num) { // Silences compiler warnings
- rclcpp::shutdown();
- exit(0);
-}
-
-int main(int argc, char * argv[])
-{
- rclcpp::init(argc, argv);
- signal(SIGINT, sigint_handler);
- rclcpp::spin(std::make_shared());
- rclcpp::shutdown();
- return 0;
-}
\ No newline at end of file
diff --git a/robossembler_interfaces/CMakeLists.txt b/robossembler_interfaces/CMakeLists.txt
index 80455a8..00ef54f 100644
--- a/robossembler_interfaces/CMakeLists.txt
+++ b/robossembler_interfaces/CMakeLists.txt
@@ -22,6 +22,8 @@ find_package(geometry_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveitSendPose.action"
+ "action/MoveitSendJointStates.action"
+ "msg/PropertyValuePair.msg"
"msg/ActionFeedbackStatusConstants.msg"
"msg/ActionResultStatusConstants.msg"
DEPENDENCIES geometry_msgs
diff --git a/robossembler_interfaces/action/MoveToJointsMoveIt.action b/robossembler_interfaces/action/MoveitSendJointStates.action
similarity index 65%
rename from robossembler_interfaces/action/MoveToJointsMoveIt.action
rename to robossembler_interfaces/action/MoveitSendJointStates.action
index 00ba6bb..6187200 100644
--- a/robossembler_interfaces/action/MoveToJointsMoveIt.action
+++ b/robossembler_interfaces/action/MoveitSendJointStates.action
@@ -1,9 +1,11 @@
##Description: Moves robot arm to a specified joint space.
#goal definition
-moveit_action_handlers/PropertyValuePair[] joint_pairs # list of joint_pairs (joint name + value)
-float32 end_effector_velocity
-float32 end_effector_acceleration
+string robot_name
+robossembler_interfaces/PropertyValuePair[] joint_pairs # list of joint_pairs (joint name + value)
+float32 joint_value # send joint value to gripper
+float32 joints_velocity_scaling_factor
+float32 joints_acceleration_scaling_factor
float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
---
#result definition
diff --git a/robossembler_interfaces/action/MoveitSendPose.action b/robossembler_interfaces/action/MoveitSendPose.action
index bc9ca78..1de1d7d 100644
--- a/robossembler_interfaces/action/MoveitSendPose.action
+++ b/robossembler_interfaces/action/MoveitSendPose.action
@@ -7,7 +7,7 @@ int32 constraint_mode
#similar to geometry_msgs/PoseStamped but using euler instead of quaternion
#at target_pose->header.frame_id define the tf frame
# which the pose will be calculated relative from
-geometry_msgs/PoseStamped target_pose
+geometry_msgs/Pose target_pose
string robot_name
float32 end_effector_velocity
float32 end_effector_acceleration
diff --git a/robossembler_servers/CMakeLists.txt b/robossembler_servers/CMakeLists.txt
index 07540fc..32efcc9 100644
--- a/robossembler_servers/CMakeLists.txt
+++ b/robossembler_servers/CMakeLists.txt
@@ -51,25 +51,19 @@ 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
-)
-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_to_joint_states_action_server src/move_to_joint_states_action_server.cpp)
+target_include_directories(move_to_joint_states_action_server PRIVATE include)
+ament_target_dependencies(move_to_joint_states_action_server ${deps})
+
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
+ TARGETS
+ move_topose_action_server
+ move_to_joint_states_action_server
DESTINATION lib/${PROJECT_NAME}
)
diff --git a/robossembler_servers/src/move_to_joint_states_action_server.cpp b/robossembler_servers/src/move_to_joint_states_action_server.cpp
new file mode 100644
index 0000000..365bae3
--- /dev/null
+++ b/robossembler_servers/src/move_to_joint_states_action_server.cpp
@@ -0,0 +1,167 @@
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/timer.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+
+// action libs
+#include "rclcpp_action/rclcpp_action.hpp"
+#include "robossembler_interfaces/action/moveit_send_joint_states.hpp"
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/quaternion.hpp"
+#include "geometry_msgs/msg/transform.hpp"
+
+// moveit libs
+#include "moveit/move_group_interface/move_group_interface.h"
+#include "moveit/planning_interface/planning_interface.h"
+
+/*
+#include
+#include
+#include
+*/
+// For Visualization
+//#include
+#include "moveit_msgs/msg/display_robot_state.hpp"
+#include "moveit_msgs/msg/display_trajectory.hpp"
+#include "moveit_msgs/msg/robot_trajectory.hpp"
+#include "moveit_msgs/action/move_group.hpp"
+
+namespace robossembler_actions
+{
+
+class MoveToJointStateActionServer : public rclcpp::Node
+{
+public:
+ using MoveitSendJointStates = robossembler_interfaces::action::MoveitSendJointStates;
+
+ //explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
+ explicit MoveToJointStateActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_to_joint_states_action_server"), node_(node)
+ {
+ // using namespace std::placeholders;
+ // this->action_server_ = rclcpp_action::create_server(
+ // 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),
+ // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
+ }
+
+ void init()
+ {
+
+ action_server_ = rclcpp_action::create_server(
+ node_->get_node_base_interface(),
+ node_->get_node_clock_interface(),
+ node_->get_node_logging_interface(),
+ node_->get_node_waitables_interface(),
+ "move_to_joint_states",
+ std::bind(&MoveToJointStateActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
+ std::bind(&MoveToJointStateActionServer::cancel_callback, this, std::placeholders::_1),
+ std::bind(&MoveToJointStateActionServer::accepted_callback, this, std::placeholders::_1)
+ );
+
+ }
+
+private:
+ rclcpp::Node::SharedPtr node_;
+ rclcpp_action::Server::SharedPtr action_server_;
+
+ using ServerGoalHandle = rclcpp_action::ServerGoalHandle;
+
+ rclcpp_action::GoalResponse goal_callback(
+ const rclcpp_action::GoalUUID & uuid,
+ std::shared_ptr goal)
+ {
+ RCLCPP_INFO(this->get_logger(), "Goal received for robot [%s]", goal->robot_name.c_str());
+ (void)uuid;
+ if (false) {
+ return rclcpp_action::GoalResponse::REJECT;
+ }
+ return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
+ }
+
+ rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle)
+ {
+ RCLCPP_INFO(this->get_logger(), "Received cancel request");
+ (void)goal_handle;
+ return rclcpp_action::CancelResponse::ACCEPT;
+ }
+
+ void accepted_callback(const std::shared_ptr goal_handle)
+ {
+ using namespace std::placeholders;
+ std::thread(std::bind(&MoveToJointStateActionServer::execute, this, _1), goal_handle).detach();
+ }
+
+ void execute(const std::shared_ptr goal_handle)
+ {
+ RCLCPP_INFO(this->get_logger(), "Executing action goal");
+ const auto goal = goal_handle->get_goal();
+ auto result = std::make_shared();
+
+ moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
+
+ const moveit::core::JointModelGroup* joint_model_group =
+ move_group_interface.getCurrentState()->getJointModelGroup(goal->robot_name);
+ moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState(10);
+
+ std::vector joint_group_positions;
+ current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);
+
+ joint_group_positions[0] = goal->joint_value;
+ joint_group_positions[1] = goal->joint_value;
+ move_group_interface.setJointValueTarget(joint_group_positions);
+ move_group_interface.setMaxVelocityScalingFactor(goal->joints_velocity_scaling_factor);
+ move_group_interface.setMaxAccelerationScalingFactor(goal->joints_acceleration_scaling_factor);
+
+
+ moveit::planning_interface::MoveGroupInterface::Plan plan;
+ bool success = (move_group_interface.plan(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+ if(success)
+ {
+ RCLCPP_INFO(this->get_logger(), "Planning success");
+ move_group_interface.execute(plan);
+ move_group_interface.move();
+ }else{
+ RCLCPP_WARN(this->get_logger(), "Failed to generate plan");
+ }
+
+ if (goal_handle->is_canceling()) {
+ goal_handle->canceled(result);
+ RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
+ return;
+ }
+
+
+ goal_handle->succeed(result);
+ RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
+ }
+}; // class MoveToJointStateActionServer
+
+}// namespace robossembler_actions
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::NodeOptions node_options;
+ node_options.automatically_declare_parameters_from_overrides(true);
+ auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
+
+ robossembler_actions::MoveToJointStateActionServer server(node);
+ std::thread run_server([&server]() {
+ rclcpp::sleep_for(std::chrono::seconds(3));
+ server.init();
+ });
+
+ rclcpp::spin(node);
+ run_server.join();
+
+ return 0;
+}
\ No newline at end of file
diff --git a/robossembler_servers/src/move_topose_action_server.cpp b/robossembler_servers/src/move_topose_action_server.cpp
index 0388ff5..e8c2e06 100644
--- a/robossembler_servers/src/move_topose_action_server.cpp
+++ b/robossembler_servers/src/move_topose_action_server.cpp
@@ -40,18 +40,34 @@ public:
using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
- explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose"), node_(node)
+ explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr &node) : Node("move_topose_action_server"), node_(node)
{
- using namespace std::placeholders;
- this->action_server_ = rclcpp_action::create_server(
- this->get_node_base_interface(),
- this->get_node_clock_interface(),
- this->get_node_logging_interface(),
- this->get_node_waitables_interface(),
+ // using namespace std::placeholders;
+ // this->action_server_ = rclcpp_action::create_server(
+ // 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),
+ // std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
+ }
+
+ void init()
+ {
+
+ action_server_ = rclcpp_action::create_server(
+ node_->get_node_base_interface(),
+ node_->get_node_clock_interface(),
+ node_->get_node_logging_interface(),
+ node_->get_node_waitables_interface(),
"move_topose",
- std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
- std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
- std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
+ std::bind(&MoveToPoseActionServer::goal_callback, this, std::placeholders::_1, std::placeholders::_2),
+ std::bind(&MoveToPoseActionServer::cancel_callback, this, std::placeholders::_1),
+ std::bind(&MoveToPoseActionServer::accepted_callback, this, std::placeholders::_1)
+ );
+
}
private:
@@ -65,9 +81,9 @@ private:
std::shared_ptr goal)
{
RCLCPP_INFO(this->get_logger(), "Received goal request for robot [%s] with Pose [%f, %f, %f, %f, %f, %f, %f]",
- 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);
+ goal->robot_name.c_str(), goal->target_pose.position.x, goal->target_pose.position.y, goal->target_pose.position.z,
+ goal->target_pose.orientation.x, goal->target_pose.orientation.y, goal->target_pose.orientation.z,
+ goal->target_pose.orientation.w);
(void)uuid;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
@@ -90,21 +106,15 @@ private:
void execute(const std::shared_ptr goal_handle)
{
- RCLCPP_INFO(this->get_logger(), "Executing goal");
+ RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared();
- 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.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);
@@ -119,13 +129,13 @@ private:
if (goal_handle->is_canceling()) {
goal_handle->canceled(result);
- RCLCPP_ERROR(this->get_logger(), "Goal Canceled");
+ RCLCPP_ERROR(this->get_logger(), "Action goal canceled");
return;
}
goal_handle->succeed(result);
- RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
+ RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
}
}; // class MoveToPoseActionServer
@@ -136,14 +146,16 @@ int main(int argc, char ** argv)
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
- auto node = rclcpp::Node::make_shared("move_topose", node_options);
- auto action_server = std::make_shared(node);
+ auto node = rclcpp::Node::make_shared("move_topose", "", node_options);
- //rclcpp::spin(action_server);
+ robossembler_actions::MoveToPoseActionServer server(node);
+ std::thread run_server([&server]() {
+ rclcpp::sleep_for(std::chrono::seconds(3));
+ server.init();
+ });
- rclcpp::executors::MultiThreadedExecutor executor;
- executor.add_node(action_server);
- executor.spin();
+ rclcpp::spin(node);
+ run_server.join();
return 0;
}
\ No newline at end of file