robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

many changes

Browse source
This commit is contained in:
Ilya Uraev 2022-01-31 21:28:39 +04:00
parent 0f172f6f4b
commit 7322505b1e
23 changed files with 499 additions and 70 deletions
Download patch file Download diff file Expand all files Collapse all files

9
robossembler/CMakeLists.txt
View file

@ -19,6 +19,7 @@ find_package(plansys2_pddl_parser REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(plansys2_bt_actions REQUIRED)
find_package(gazebo_msgs REQUIRED)
find_package(robossembler_interfaces REQUIRED)
if (NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
@ -41,14 +42,18 @@ set(dependencies
ament_index_cpp
plansys2_bt_actions
gazebo_msgs
robossembler_interfaces
)
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_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_move_bt_node)
list(APPEND plugin_libs robossembler_emu_pose_estimation)
add_library(robossembler_move_gripper_bt_node SHARED src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp)
@ -70,7 +75,7 @@ install(TARGETS
${plugin_libs}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}
RUNTIME DESTINATION bin
)
if(BUILD_TESTING)

11
robossembler/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml
View file

@ -3,22 +3,21 @@
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action ID="EmuPoseEstimation" entity_name="${arg1}" part_pose="${p_pose}"/>
<Action ID="Move" arm_group="${arg0}" goal="${goal}"/>
<Action ID="MoveGripper" gripper_group="${arg2}" relative_gap="${gap}"/>
<Action ID="EmuPoseEstimation" PartName="${arg1}"/>
<Action ID="MoveToPose" arm_group="${arg0}" goal="{goal}" />
<Action ID="MoveGripper" gripper_group="ramst_hand_arm_group" relative_gap="open" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="EmuPoseEstimation">
<input_port name="entity_name"/>
<output_port name="part_pose"/>
<input_port name="PartName"/>
</Action>
<Action ID="MoveGripper">
<input_port name="gripper_group"/>
<input_port name="relative_gap"/>
</Action>
<Action ID="Move">
<Action ID="MoveToPose">
<input_port name="arm_group"/>
<input_port name="goal"/>
</Action>

2
robossembler/config/params.yaml
View file

@ -1,4 +1,4 @@
pick_and_place:
assemble_1:
ros__parameters:
plugins:
- robossembler_move_bt_node

2
robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.hpp
View file

@ -30,7 +30,7 @@ namespace task_planner
*/
static BT::PortsList providedPorts()
{
return {BT::InputPort<std::string>("PartPose")};
return {BT::InputPort<std::string>("PartName")};
}
private:

4
robossembler/launch/task_planner.launch.py
View file

@ -86,7 +86,7 @@ def generate_launch_description():
assemble_1 = Node(
package='plansys2_bt_actions',
executable='bt_action_node',
name='pick_and_place',
name='assemble_1',
namespace=namespace,
output='screen',
parameters=[
@ -95,7 +95,7 @@ def generate_launch_description():
robot_description_semantic,
kinematics_yaml,
{
'action_name': 'pick_and_place',
'action_name': 'assemble-1',
'publisher_port': 1666,
'server_port': 1667,
'bt_xml_file': pkg_dir + '/behavior_trees_xml/atomic_skills_xml/simple_sequence.xml',

1
robossembler/package.xml
View file

@ -28,6 +28,7 @@
<depend>plansys2_pddl_parser</depend>
<depend>plansys2_bt_actions</depend>
<depend>ament_index_cpp</depend>
<depend>robossembler_interfaces</depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_lint_auto</test_depend>

11
robossembler/pddl/domain.pddl
View file

@ -6,15 +6,16 @@
robot
zone
gap
entity
part
gripper
);; end Types ;;;;;;;;;;;;;;;;;;;;;;;;;
;; Predicates ;;;;;;;;;;;;;;;;;;;;;;;;;
(:predicates
(move_at ?r - robot ?z - zone)
(part_pose ?e - entity)
(part_pose ?p - part)
(gripper_move ?gr - gripper ?g - gap)
(part_at ?r - robot ?p - part)
);; end Predicates ;;;;;;;;;;;;;;;;;;;;
;; Functions ;;;;;;;;;;;;;;;;;;;;;;;;;
@ -24,9 +25,9 @@
;; Actions ;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(:durative-action assemble-1
:parameters (?r - robot ?z - zone ?e - entity ?g - gap ?gr - gripper)
:duration ( = ?duration 5)
:effect (and(at end (move_at ?r ?z))(at end (part_pose ?e))(at end (gripper_move ?gr ?g)))
:parameters (?r - robot ?p - part)
:duration ( = ?duration 1)
:effect (and(at start (part_at ?r ?p))
)
;(:durative-action move_gripper

15
robossembler/src/behavior_tree_nodes/atomic_skills/EmuPoseEstimation.cpp
View file

@ -9,7 +9,7 @@
namespace task_planner
{
static const rclcpp::Logger LOGGER = rclcpp::get_logger("gz_get_pose");
static const rclcpp::Logger LOGGER = rclcpp::get_logger("PartPoseEstimation");
EmuPoseEstimation::EmuPoseEstimation(const std::string &xml_tag_name,
const BT::NodeConfiguration &conf)
@ -17,18 +17,18 @@ namespace task_planner
{
rclcpp::Node::SharedPtr node;
config().blackboard->get("node", node);
RCLCPP_INFO(LOGGER, "EmuPoseEstimation constructor started");
try
{
node->declare_parameter("PartPose");
node->declare_parameter("PartName");
}
catch(const rclcpp::exceptions::ParameterAlreadyDeclaredException &ex)
{
RCLCPP_WARN(LOGGER, "%c", ex.what());
}
if (node->has_parameter("PartPose"))
if (node->has_parameter("PartName"))
{
node->get_parameter_or("PartPose", available_model_name_, {});
node->get_parameter_or("PartName", available_model_name_, {});
}else
{
RCLCPP_WARN(LOGGER, "No model_name provided");
@ -45,6 +45,7 @@ namespace task_planner
{
rclcpp::Node::SharedPtr gz_get_entity_state;
config().blackboard->get("node", gz_get_entity_state);
RCLCPP_INFO(LOGGER, "EmuPoseEstimation action started");
rclcpp::Client<gazebo_msgs::srv::GetEntityState>::SharedPtr client =
gz_get_entity_state->create_client
@ -55,7 +56,7 @@ namespace task_planner
std::thread([&executor]() {executor.spin();}).detach();
std::string gazebo_model_name;
getInput<std::string>("PartPose",gazebo_model_name);
getInput<std::string>("PartName",gazebo_model_name);
if(std::find(available_model_name_.begin(), available_model_name_.end(), gazebo_model_name) != available_model_name_.end())
{
@ -68,7 +69,7 @@ namespace task_planner
auto request = std::make_shared<gazebo_msgs::srv::GetEntityState::Request>();
request->name=model_name_;
RCLCPP_INFO(LOGGER, "Provided service call with model name [%s]", request->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()) {

31
robossembler/src/behavior_tree_nodes/atomic_skills/GetEntityState.cpp Normal file
View file

@ -0,0 +1,31 @@
#include "gazebo_msgs/srv/get_entity_state.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtService.hpp"
using namespace BT;
using GetEntityStateSrv = gazebo_msgs::srv::GetEntityState;
class GetEntityState : public BtService<GetEntityStateSrv>
{
public:
GetEntityState(const std::string & name, const BT::NodeConfiguration & config)
: BtService<GetEntityStateSrv>(name, config){
}
GetEntityStateSrv::Request::SharedPtr populate_request() override
{
GetEntityStateSrv::Request::SharedPtr request;
return request;
}
BT::NodeStatus handle_response(GetEntityStateSrv::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "Service call complete");
return BT::NodeStatus::SUCCESS;
}
};
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<GetEntityStateSrv>("GetEntityState");
}

4
robossembler/src/behavior_tree_nodes/atomic_skills/Move.cpp
View file

@ -22,7 +22,7 @@ namespace task_planner
{
rclcpp::Node::SharedPtr node;
config().blackboard->get("node", node);
RCLCPP_INFO(LOGGER, "Move consructor started");
try
{
node->declare_parameter("arm_group");
@ -92,7 +92,7 @@ namespace task_planner
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);

2
robossembler/src/behavior_tree_nodes/atomic_skills/MoveGripper.cpp
View file

@ -22,6 +22,7 @@ namespace task_planner
{
rclcpp::Node::SharedPtr node;
config().blackboard->get("node", node);
RCLCPP_INFO(LOGGER, "MoveGripper constructor started");
try
{
@ -76,6 +77,7 @@ namespace task_planner
BT::NodeStatus MoveGripper::tick()
{
RCLCPP_INFO(LOGGER, "MoveGripper action started");
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);

48
robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp Normal file
View file

@ -0,0 +1,48 @@
#include "robossembler_interfaces/action/moveit_send_pose.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
using namespace BT;
using MoveToPoseAction = robossembler_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPointActionClient");
class MoveToPose : public BtAction<MoveToPoseAction>
{
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config) {
available_groups_ = getInput<std::string>("arm_group").value();
target_pose_ = getInput<geometry_msgs::msg::PoseStamped>("goal").value();
}
MoveToPoseAction::Goal populate_goal() override
{
MoveToPoseAction::Goal goal;
goal.robot_name = available_groups_;
goal.end_effector_acceleration = 1;
goal.end_effector_velocity = 1;
goal.target_pose = target_pose_;
return goal;
}
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("arm_group"),
InputPort<geometry_msgs::msg::PoseStamped>("goal")});
}
private:
std::string available_groups_;
std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
geometry_msgs::msg::PoseStamped target_pose_;
std::string arm_group_;
}; // class MoveToPose
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToPose>("MoveToPose");
}

89
robossembler/src/move_controller_node.cpp
View file

@ -25,26 +25,43 @@ public:
{
}
void init()
bool init()
{
domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
planner_client_ = std::make_shared<plansys2::PlannerClient>();
problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
executor_client_ = std::make_shared<plansys2::ExecutorClient>();
init_knowledge();
auto domain = domain_expert_->getDomain();
auto problem = problem_expert_->getProblem();
auto plan = planner_client_->getPlan(domain, problem);
if (!plan.has_value()) {
std::cout << "Could not find plan to reach goal " <<
parser::pddl::toString(problem_expert_->getGoal()) << std::endl;
return false;
}
if (!executor_client_->start_plan_execution(plan.value()))
{
RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)");
}
return true;
}
void init_knowledge()
{
problem_expert_->addInstance(plansys2::Instance{"rasmt_arm_group", "robot"});
problem_expert_->addInstance(plansys2::Instance{"rasmt_hand_arm_gour", "gripper"});
problem_expert_->addInstance(plansys2::Instance{"rasmt","entity"});
//problem_expert_->addPredicate(plansys2::Predicate("(pose-estimated rasmt)"));
//problem_expert_->addPredicate(plansys2::Predicate("robot_move rasmt_arm_group one"));
problem_expert_->addInstance(plansys2::Instance{"rasmt", "part"});
problem_expert_->setGoal(plansys2::Goal("(and(part_at rasmt_arm_group rasmt)))"));
}
std::string status_to_string(int8_t status)
/*std::string status_to_string(int8_t status)
{
switch (status)
{
@ -67,41 +84,20 @@ public:
return "UNKNOWN";
break;
}
}
}*/
void step()
{
problem_expert_->setGoal(plansys2::Goal(""));
{
if (!executor_client_->execute_and_check_plan()) { // Plan finished
auto result = executor_client_->getResult();
auto domain = domain_expert_->getDomain();
auto problem = problem_expert_->getProblem();
auto plan = planner_client_->getPlan(domain, problem);
if (!plan.has_value()) {
std::cout << "Could not find plan to reach goal " <<
parser::pddl::toString(problem_expert_->getGoal()) << std::endl;
}
executor_client_->start_plan_execution(plan.value());
auto feedback = executor_client_->getFeedBack();
for (const auto & action_feedback : feedback.action_execution_status)
{
std::cout << "[" << action_feedback.action << " " <<
action_feedback.completion * 100.0 << "%]";
}
std::cout << std::endl;
if (!executor_client_->execute_and_check_plan() && executor_client_->getResult())
{
if (executor_client_->getResult().value().success)
{
std::cout << "Successful finished " << std::endl;
}
if (result.value().success) {
RCLCPP_INFO(get_logger(), "Plan succesfully finished");
} else {
RCLCPP_ERROR(get_logger(), "Plan finished with error");
}
}
}
private:
std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
@ -115,16 +111,21 @@ int main(int argc, char ** argv)
rclcpp::init(argc, argv);
auto node = std::make_shared<MoveController>();
node->init();
rclcpp::Rate rate(5);
node->step();
//node->init();
if (!node->init())
{
RCLCPP_ERROR(node->get_logger(), "Error in Init");
return 0;
}
rate.sleep();
rclcpp::spin_some(node->get_node_base_interface());
node->step();
rclcpp::Rate rate(5);
while (rclcpp::ok()) {
node->step();
rate.sleep();
rclcpp::spin_some(node->get_node_base_interface());
}
rate.sleep();
rclcpp::shutdown();
return 0;

33
robossembler_interfaces/CMakeLists.txt Normal file
View file

@ -0,0 +1,33 @@
cmake_minimum_required(VERSION 3.5)
project(robossembler_interfaces)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/MoveitSendPose.action"
"msg/ActionFeedbackStatusConstants.msg"
"msg/ActionResultStatusConstants.msg"
)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

17
robossembler_interfaces/action/MoveToJointsMoveIt.action Normal file
View file

@ -0,0 +1,17 @@
##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
float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
---
#result definition
bool success
uint64 millis_passed
string status #Use the constants of ActionResultStatusConstants in the status field
---
#feedback
bool success
uint64 millis_passed
string status #Use the constants of ActionFeedbackStatusConstants in the status field

24
robossembler_interfaces/action/MoveitSendPose.action Normal file
View file

@ -0,0 +1,24 @@
##Description: Moves robot arm to a specified pose relative to the frame in header.frame_id of target_pose
#goal definition
#Used to indicate which hardcoded motion constraint to use
#e.g 0 no constraint, 1 keep the same end effector orientation
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
string robot_name
float32 end_effector_velocity
float32 end_effector_acceleration
float32 timeout_seconds #if this action cannot be completed within this time period it should be considered failed.
---
#result definition
bool success
uint64 millis_passed
string status #Use the constants of ActionResultStatusConstants in the status field
---
#feedback
bool success
uint64 millis_passed
string status #Use the constants of ActionFeedbackStatusConstants in the status field

5
robossembler_interfaces/msg/ActionFeedbackStatusConstants.msg Normal file
View file

@ -0,0 +1,5 @@
string PLANNING="Planning"
string EXECUTING="Executing"
string EMERGENCY_STOP="EmergencyStop"
string OPERATIONAL_EXCEPTION="OperationalException"
string CANCELLING="Cancelling"

6
robossembler_interfaces/msg/ActionResultStatusConstants.msg Normal file
View file

@ -0,0 +1,6 @@
string SUCCESS="Success"
string PLANNING_FAILED="PlanningFailed"
string CONTROL_FAILED="ControlFailed"
string EMERGENCY_STOP="EmergencyStop"
string OPERATIONAL_EXCEPTION="OperationalException"
string CANCELLED="Cancelled"

5
robossembler_interfaces/msg/PropertyValuePair.msg Normal file
View file

@ -0,0 +1,5 @@
##Usage: In MoveToJointsMoveIt.action
##Definition: A property name - Value definition.
string name
float64 value

23
robossembler_interfaces/package.xml Normal file
View file

@ -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>robossembler_interfaces</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

73
robossembler_servers/CMakeLists.txt Normal file
View file

@ -0,0 +1,73 @@
cmake_minimum_required(VERSION 3.5)
project(robossembler_servers)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(robossembler_interfaces REQUIRED)
set(deps
rclcpp
rclcpp_action
moveit_core
moveit_ros_planning
moveit_ros_planning_interface
moveit_msgs
geometry_msgs
tf2_ros
rclcpp_components
robossembler_interfaces
)
#
include_directories(include)
install(DIRECTORY include
DESTINATION include
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
ament_target_dependencies(move_topose_action ${deps})
rclcpp_components_register_node(move_topose_action PLUGIN "robossembler_actions::MoveToPoseActionServer" EXECUTABLE move_topose_action_server)
install(
TARGETS move_topose_action
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
ament_package()

28
robossembler_servers/package.xml Normal file
View file

@ -0,0 +1,28 @@
<?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>robossembler_servers</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="ur.narmak@gmail.com">bill-finger</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_msgs</depend>
<depend>tf2_ros</depend>
<depend>rclcpp_action</depend>
<depend>geometry_msgs</depend>
<depend>action_msgs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

126
robossembler_servers/src/move_topose_action_server.cpp Normal file
View file

@ -0,0 +1,126 @@
#include <functional>
#include <memory>
#include <thread>
#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/msg/action_feedback_status_constants.hpp"
#include "robossembler_interfaces/action/moveit_send_pose.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 <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
*/
// For Visualization
//#include <eigen_conversions/eigen_msg.h>
#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 MoveToPoseActionServer : public rclcpp::Node
{
public:
using MoveitSendPose = robossembler_interfaces::action::MoveitSendPose;
explicit MoveToPoseActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
: Node("move_topose_action_server", options)
{
using namespace std::placeholders;
this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
this,
"MoveToPoseActionServer",
std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
}
private:
rclcpp::Node::SharedPtr node_;
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->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;
if (false) {
return rclcpp_action::GoalResponse::REJECT;
}
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}
rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr<ServerGoalHandle> 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<ServerGoalHandle> goal_handle)
{
using namespace std::placeholders;
std::thread(std::bind(&MoveToPoseActionServer::execute, this, _1), goal_handle).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendPose::Result>();
moveit::planning_interface::MoveGroupInterface::Options move_group_options(goal->robot_name, "robot_description");
moveit::planning_interface::MoveGroupInterface move_group_interface(node_, goal->robot_name);
moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
move_group_interface.setStartState(start_state);
move_group_interface.setPoseTarget(goal->target_pose.pose);
move_group_interface.setMaxVelocityScalingFactor(goal->end_effector_velocity);
move_group_interface.setMaxAccelerationScalingFactor(goal->end_effector_acceleration);
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(), "Goal Canceled");
return;
}
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed goal");
}
}; // class MoveToPoseActionServer
}// namespace robossembler_actions
RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_actions::MoveToPoseActionServer)