🔨 add bt_checker and BT node with new lib
This commit is contained in:
parent
7322505b1e
commit
7ca3fbeb1d
10 changed files with 191 additions and 35 deletions
|
@ -156,9 +156,28 @@ def generate_launch_description():
|
|||
]
|
||||
)
|
||||
|
||||
move_topose_action_server = Node(
|
||||
package="robossembler_servers",
|
||||
executable="move_topose_action_server",
|
||||
name="m2p_as",
|
||||
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
|
||||
]
|
||||
)
|
||||
|
||||
launch_nodes = []
|
||||
launch_nodes.append(rviz)
|
||||
launch_nodes.append(move_group_node)
|
||||
launch_nodes.append(move_topose_action_server)
|
||||
|
||||
|
||||
|
||||
|
|
|
@ -20,6 +20,7 @@ find_package(ament_index_cpp REQUIRED)
|
|||
find_package(plansys2_bt_actions REQUIRED)
|
||||
find_package(gazebo_msgs REQUIRED)
|
||||
find_package(robossembler_interfaces REQUIRED)
|
||||
find_package(behavior_tree REQUIRED)
|
||||
|
||||
if (NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
|
@ -43,6 +44,8 @@ set(dependencies
|
|||
plansys2_bt_actions
|
||||
gazebo_msgs
|
||||
robossembler_interfaces
|
||||
behavior_tree
|
||||
std_msgs
|
||||
)
|
||||
|
||||
include_directories(include)
|
||||
|
@ -50,6 +53,9 @@ 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_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)
|
||||
|
||||
|
@ -67,6 +73,9 @@ 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})
|
||||
|
||||
|
@ -78,6 +87,10 @@ 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()
|
||||
|
|
|
@ -3,23 +3,17 @@
|
|||
<!-- ////////// -->
|
||||
<BehaviorTree ID="BehaviorTree">
|
||||
<Sequence>
|
||||
<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" />
|
||||
<Action ID="GetEntityState" server_name="/get_entity_state" server_timeout="10" PartName="rasmt"/>
|
||||
<Action ID="MoveToPose" server_name="/move_topose" server_timeout="10" cancel_timeout="5" arm_group="${arg0}" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
<!-- ////////// -->
|
||||
<TreeNodesModel>
|
||||
<Action ID="EmuPoseEstimation">
|
||||
<Action ID="GetEntityState">
|
||||
<input_port name="PartName"/>
|
||||
</Action>
|
||||
<Action ID="MoveGripper">
|
||||
<input_port name="gripper_group"/>
|
||||
<input_port name="relative_gap"/>
|
||||
</Action>
|
||||
<Action ID="MoveToPose">
|
||||
<input_port name="arm_group"/>
|
||||
<input_port name="goal"/>
|
||||
</Action>
|
||||
</TreeNodesModel>
|
||||
<!-- ////////// -->
|
||||
|
|
|
@ -1,10 +1,9 @@
|
|||
assemble_1:
|
||||
ros__parameters:
|
||||
plugins:
|
||||
- robossembler_move_bt_node
|
||||
- robossembler_emu_pose_estimation
|
||||
- robossembler_move_gripper_bt_node
|
||||
PartName: ["rasmt"]
|
||||
- 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"]
|
||||
|
|
20
robossembler/launch/bt_check.launch.py
Normal file
20
robossembler/launch/bt_check.launch.py
Normal file
|
@ -0,0 +1,20 @@
|
|||
from launch import LaunchDescription
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
import os
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
return LaunchDescription([
|
||||
Node(
|
||||
package='robossembler',
|
||||
namespace='',
|
||||
executable='bt_check',
|
||||
# Do not declare a node name otherwise it messes with the action node names and will result in
|
||||
# duplicate nodes!
|
||||
parameters=[
|
||||
{'bt_file_path': os.path.join(get_package_share_directory('robossembler'), 'behavior_trees_xml/atomic_skills_xml/simple_sequence.xml')},
|
||||
{'plugins': ['robossembler_get_entity_state_bt_action_client', 'robossembler_move_topose_bt_action']}
|
||||
]
|
||||
),
|
||||
])
|
|
@ -10,12 +10,18 @@ class GetEntityState : public BtService<GetEntityStateSrv>
|
|||
public:
|
||||
GetEntityState(const std::string & name, const BT::NodeConfiguration & config)
|
||||
: BtService<GetEntityStateSrv>(name, config){
|
||||
|
||||
|
||||
//getInput<std::string>("PartName", part_name_);
|
||||
part_name_ = getInput<std::string>("PartName").value();
|
||||
RCLCPP_INFO_STREAM(_node->get_logger(), "Provided part name ["<< part_name_<<"]");
|
||||
}
|
||||
|
||||
GetEntityStateSrv::Request::SharedPtr populate_request() override
|
||||
{
|
||||
GetEntityStateSrv::Request::SharedPtr request;
|
||||
//GetEntityStateSrv::Request::SharedPtr request;
|
||||
auto request = std::make_shared<GetEntityStateSrv::Request>();
|
||||
request->name = part_name_;
|
||||
RCLCPP_INFO(_node->get_logger(), "Request populated");
|
||||
return request;
|
||||
}
|
||||
BT::NodeStatus handle_response(GetEntityStateSrv::Response::SharedPtr response) override
|
||||
|
@ -23,9 +29,17 @@ class GetEntityState : public BtService<GetEntityStateSrv>
|
|||
RCLCPP_INFO(_node->get_logger(), "Service call complete");
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
|
||||
static PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts({
|
||||
InputPort<std::string>("PartName")});
|
||||
}
|
||||
private:
|
||||
std::string part_name_;
|
||||
};
|
||||
|
||||
BT_REGISTER_NODES(factory)
|
||||
{
|
||||
factory.registerNodeType<GetEntityStateSrv>("GetEntityState");
|
||||
factory.registerNodeType<GetEntityState>("GetEntityState");
|
||||
}
|
|
@ -12,14 +12,18 @@ class MoveToPose : public BtAction<MoveToPoseAction>
|
|||
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();
|
||||
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;
|
||||
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str());
|
||||
}
|
||||
|
||||
MoveToPoseAction::Goal populate_goal() override
|
||||
{
|
||||
MoveToPoseAction::Goal goal;
|
||||
goal.robot_name = available_groups_;
|
||||
//auto goal = std::make_shared<MoveToPoseAction::Goal>();
|
||||
goal.robot_name = robot_name_;
|
||||
goal.end_effector_acceleration = 1;
|
||||
goal.end_effector_velocity = 1;
|
||||
goal.target_pose = target_pose_;
|
||||
|
@ -30,15 +34,13 @@ class MoveToPose : public BtAction<MoveToPoseAction>
|
|||
static PortsList providedPorts()
|
||||
{
|
||||
return providedBasicPorts({
|
||||
InputPort<std::string>("arm_group"),
|
||||
InputPort<geometry_msgs::msg::PoseStamped>("goal")});
|
||||
InputPort<std::string>("arm_group")});
|
||||
}
|
||||
|
||||
private:
|
||||
std::string available_groups_;
|
||||
std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
|
||||
//std::map<std::string, geometry_msgs::msg::Pose> waypoints_;
|
||||
geometry_msgs::msg::PoseStamped target_pose_;
|
||||
std::string arm_group_;
|
||||
std::string robot_name_;
|
||||
|
||||
}; // class MoveToPose
|
||||
|
||||
|
|
77
robossembler/src/bt_checker.cpp
Normal file
77
robossembler/src/bt_checker.cpp
Normal file
|
@ -0,0 +1,77 @@
|
|||
|
||||
#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");
|
||||
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;
|
||||
}
|
|
@ -51,7 +51,7 @@ if(BUILD_TESTING)
|
|||
find_package(ament_lint_auto REQUIRED)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
#
|
||||
add_library(move_topose_action SHARED
|
||||
src/move_topose_action_server.cpp
|
||||
)
|
||||
|
@ -63,8 +63,13 @@ ament_target_dependencies(move_topose_action ${deps})
|
|||
|
||||
rclcpp_components_register_node(move_topose_action PLUGIN "robossembler_actions::MoveToPoseActionServer" EXECUTABLE move_topose_action_server)
|
||||
|
||||
#[[
|
||||
add_executable(move_topose_action_server src/move_topose_action_server.cpp)
|
||||
target_include_directories(move_topose_action_server PRIVATE include)
|
||||
ament_target_dependencies(move_topose_action_server ${deps})
|
||||
]]
|
||||
install(
|
||||
TARGETS move_topose_action
|
||||
TARGETS move_topose_action_server
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
|
|
|
@ -11,13 +11,13 @@
|
|||
#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>
|
||||
#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 "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>
|
||||
|
@ -38,13 +38,14 @@ 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)
|
||||
//explicit MoveToPoseActionServer(const rclcpp::Node::SharedPtr& node)
|
||||
explicit MoveToPoseActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
|
||||
: Node("move_topose_action_server", options)//, node_(node)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
this->action_server_ = rclcpp_action::create_server<MoveitSendPose>(
|
||||
this,
|
||||
"MoveToPoseActionServer",
|
||||
"move_topose",
|
||||
std::bind(&MoveToPoseActionServer::goal_callback, this, _1, _2),
|
||||
std::bind(&MoveToPoseActionServer::cancel_callback, this, _1),
|
||||
std::bind(&MoveToPoseActionServer::accepted_callback, this, _1));
|
||||
|
@ -123,4 +124,16 @@ private:
|
|||
|
||||
}// namespace robossembler_actions
|
||||
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_actions::MoveToPoseActionServer)
|
||||
RCLCPP_COMPONENTS_REGISTER_NODE(robossembler_actions::MoveToPoseActionServer)
|
||||
|
||||
/*
|
||||
int main(int argc, char * argv[])
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions node_options;
|
||||
node_options.automatically_declare_parameters_from_overrides(true);
|
||||
rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("move_topose", "", node_options);
|
||||
rclcpp::spin(node);
|
||||
//rclcpp::shutdown();
|
||||
return 0;
|
||||
}*/
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue