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

♻️ update to moveit2 ros2 actions bt

Browse source
This commit is contained in:
Ilya Uraev 2022-02-10 23:15:37 +04:00
parent 7b73c48f03
commit 54da56c7ae
18 changed files with 305 additions and 742 deletions
Download patch file Download diff file Expand all files Collapse all files

21
rasmt_moveit_config/launch/rasmt_moveit.launch.py
View file

@ -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)

21
robossembler/CMakeLists.txt
View file

@ -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()

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

@ -3,8 +3,9 @@
<!-- ////////// -->
<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="rasmt_arm_group" />
<Action ID="GetEntityState" PartName="rasmt" server_name="/get_entity_state" server_timeout="10"/>
<Action ID="MoveToPose" arm_group="rasmt_arm_group" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveGripper" arm_group="rasmt_hand_arm_group" server_name="/move_to_joint_states" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->

15
robossembler/config/params.yaml
View file

@ -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

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

@ -1,41 +0,0 @@
#pragma once
#include <string>
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <gazebo_msgs/srv/get_entity_state.hpp>
#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<std::string>("PartName")};
}
private:
std::vector<std::string> available_model_name_;
std::atomic_bool _halt_request;
std::string model_name_;
};
}

55
robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/Move.hpp
View file

@ -1,55 +0,0 @@
#pragma once
#include <string>
#include <vector>
#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<std::string>("arm_group"),
BT::InputPort<std::string>("goal")
};
}
private:
std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
std::vector<std::string> available_groups_;
std::string arm_group_;
std::atomic_bool _halt_requested;
};
} // namespace task_planner

56
robossembler/include/robot_bt/behavior_tree_nodes/atomic_skills/MoveGripper.hpp
View file

@ -1,56 +0,0 @@
#pragma once
#include <string>
#include <vector>
#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<std::string>("gripper_group"),
BT::InputPort<std::string>("relative_gap")
};
}
private:
const double max_gripper_gap_ = 0.04;
std::map<std::string, double> relative_gaps_;
std::vector<std::string> available_groups_;
std::string gripper_group_;
std::atomic_bool _halt_requested;
};
} // namespace task_planner

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

@ -1,94 +0,0 @@
#include <iostream>
#include <rclcpp/executors/single_threaded_executor.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/qos.hpp>
#include <thread>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#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<gazebo_msgs::srv::GetEntityState>::SharedPtr client =
gz_get_entity_state->create_client
<gazebo_msgs::srv::GetEntityState>("/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<std::string>("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<gazebo_msgs::srv::GetEntityState::Request>();
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<task_planner::EmuPoseEstimation>("EmuPoseEstimation");
}

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

@ -1,179 +0,0 @@
#include <iostream>
#include <thread>
#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<std::string> 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<double> 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<std::string>("goal", goal);
std::string arm_group;
getInput<std::string>("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<task_planner::Move>("Move");
}

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

@ -1,154 +1,50 @@
#include <iostream>
#include <thread>
#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<std::string> 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<std::string>("gripper_group", gripper_group);
std::string gap;
getInput<std::string>("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<MoveToJointStateAction>
{
public:
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToJointStateAction>(name, config) {
robot_name_ = getInput<std::string>("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<std::string>("arm_group")});
}
private:
double gripper_gap{0};
std::string robot_name_;
}; // class MoveToJointState
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<task_planner::MoveGripper>("MoveGripper");
}
factory.registerNodeType<MoveToJointState>("MoveGripper");
}

12
robossembler/src/behavior_tree_nodes/atomic_skills/MoveToPose.cpp
View file

@ -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<MoveToPoseAction>
: BtAction<MoveToPoseAction>(name, config) {
robot_name_ = getInput<std::string>("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<MoveToPoseAction::Goal>();
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<MoveToPoseAction>
}
private:
//std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
geometry_msgs::msg::PoseStamped target_pose_;
geometry_msgs::msg::Pose target_pose_;
std::string robot_name_;
}; // class MoveToPose

79
robossembler/src/bt_checker.cpp
View file

@ -1,79 +0,0 @@
#include "behavior_tree/BtEngine.hpp"
#include <csignal>
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<std::string>());
// 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<rclcpp::Node::SharedPtr>("node", std::make_shared<rclcpp::Node>("bt_node"));
// blackboard->set<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer", tf_);
RCLCPP_INFO(this->get_logger(), "Loading tree from file: " + bt_file_path_);
tree_ = std::make_shared<Tree>(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<BT::PublisherZMQ>(
*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<BtEngine>());
rclcpp::shutdown();
return 0;
}

2
robossembler_interfaces/CMakeLists.txt
View file

@ -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

8
robossembler_interfaces/action/MoveToJointsMoveIt.action → robossembler_interfaces/action/MoveitSendJointStates.action
View file

@ -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

2
robossembler_interfaces/action/MoveitSendPose.action
View file

@ -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

20
robossembler_servers/CMakeLists.txt
View file

@ -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
$<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)
]]
#
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}
)

167
robossembler_servers/src/move_to_joint_states_action_server.cpp Normal file
View file

@ -0,0 +1,167 @@
#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/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 <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 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<MoveitSendPose>(
// 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<MoveitSendJointStates>(
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<MoveitSendJointStates>::SharedPtr action_server_;
using ServerGoalHandle = rclcpp_action::ServerGoalHandle<MoveitSendJointStates>;
rclcpp_action::GoalResponse goal_callback(
const rclcpp_action::GoalUUID & uuid,
std::shared_ptr<const MoveitSendJointStates::Goal> 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<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(&MoveToJointStateActionServer::execute, this, _1), goal_handle).detach();
}
void execute(const std::shared_ptr<ServerGoalHandle> goal_handle)
{
RCLCPP_INFO(this->get_logger(), "Executing action goal");
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<MoveitSendJointStates::Result>();
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<double> 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;
}

72
robossembler_servers/src/move_topose_action_server.cpp
View file

@ -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<MoveitSendPose>(
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<MoveitSendPose>(
// 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<MoveitSendPose>(
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<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.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<ServerGoalHandle> 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<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.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<robossembler_actions::MoveToPoseActionServer>(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;
}