From 764d1e181fb6cfd4d92f62e2b26e27c83634a643 Mon Sep 17 00:00:00 2001 From: roi Date: Mon, 22 Apr 2024 14:17:22 +0200 Subject: [PATCH] Pan wip --- bt_nodes/bt_test/bt_xml/receptionist.xml | 175 ++++++++++++++++++ bt_nodes/motion/CMakeLists.txt | 3 + .../ctrl_support/BTActionNode.hpp | 19 +- .../motion/{navigation => head}/LookAt.hpp | 26 ++- bt_nodes/motion/include/motion/head/Pan.hpp | 60 ++++++ .../include/motion/navigation/MoveTo.hpp | 4 +- bt_nodes/motion/src/motion/head/LookAt.cpp | 53 ++---- bt_nodes/motion/src/motion/head/Pan.cpp | 89 +++++++++ .../motion/src/motion/navigation/MoveTo.cpp | 2 +- 9 files changed, 380 insertions(+), 51 deletions(-) create mode 100644 bt_nodes/bt_test/bt_xml/receptionist.xml rename bt_nodes/motion/include/{motion/navigation => }/ctrl_support/BTActionNode.hpp (95%) rename bt_nodes/motion/include/motion/{navigation => head}/LookAt.hpp (74%) create mode 100644 bt_nodes/motion/include/motion/head/Pan.hpp create mode 100644 bt_nodes/motion/src/motion/head/Pan.cpp diff --git a/bt_nodes/bt_test/bt_xml/receptionist.xml b/bt_nodes/bt_test/bt_xml/receptionist.xml new file mode 100644 index 0000000..f6bdcd1 --- /dev/null +++ b/bt_nodes/bt_test/bt_xml/receptionist.xml @@ -0,0 +1,175 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Number of guests introduced + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Number of guests introduced + + + + + array of frames + value in meters + + [depth_nearest, left_to_right]^ + + + + + + the tf frame of the object to navigate + + + + + + What the robot understood + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + what human said + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/bt_nodes/motion/CMakeLists.txt b/bt_nodes/motion/CMakeLists.txt index 8e3ee21..dab3c0e 100644 --- a/bt_nodes/motion/CMakeLists.txt +++ b/bt_nodes/motion/CMakeLists.txt @@ -34,6 +34,9 @@ list(APPEND plugin_libs move_to_bt_node) add_library(look_at_bt_node SHARED src/motion/head/LookAt.cpp) list(APPEND plugin_libs look_at_bt_node) +add_library(pan_bt_node SHARED src/motion/head/Pan.cpp) +list(APPEND plugin_libs pan_bt_node) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/bt_nodes/motion/include/motion/navigation/ctrl_support/BTActionNode.hpp b/bt_nodes/motion/include/ctrl_support/BTActionNode.hpp similarity index 95% rename from bt_nodes/motion/include/motion/navigation/ctrl_support/BTActionNode.hpp rename to bt_nodes/motion/include/ctrl_support/BTActionNode.hpp index 2ce42e4..a3e8807 100644 --- a/bt_nodes/motion/include/motion/navigation/ctrl_support/BTActionNode.hpp +++ b/bt_nodes/motion/include/ctrl_support/BTActionNode.hpp @@ -22,7 +22,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -namespace navigation +namespace motion { using namespace std::chrono_literals; // NOLINT @@ -234,6 +234,10 @@ class BtActionNode : public BT::ActionNodeBase } }; + send_goal_options.feedback_callback = + std::bind(&BtActionNode::feedback_callback, this, + std::placeholders::_1, std::placeholders::_2); + auto future_goal_handle = action_client_->async_send_goal(goal_, send_goal_options); if (rclcpp::spin_until_future_complete( @@ -248,6 +252,19 @@ class BtActionNode : public BT::ActionNodeBase throw std::runtime_error("Goal was rejected by the action server"); } } + + void feedback_callback( + typename rclcpp_action::ClientGoalHandle::SharedPtr, + const typename ActionT::Feedback::ConstSharedPtr feedback) + { + config().blackboard->set("feedback", feedback); + on_feedback(); + } + + virtual void on_feedback() + { + + } void increment_recovery_count() { diff --git a/bt_nodes/motion/include/motion/navigation/LookAt.hpp b/bt_nodes/motion/include/motion/head/LookAt.hpp similarity index 74% rename from bt_nodes/motion/include/motion/navigation/LookAt.hpp rename to bt_nodes/motion/include/motion/head/LookAt.hpp index 6693aec..ec1b365 100644 --- a/bt_nodes/motion/include/motion/navigation/LookAt.hpp +++ b/bt_nodes/motion/include/motion/head/LookAt.hpp @@ -15,28 +15,27 @@ #ifndef NAVIGATION__LOOK_AT_HPP_ #define NAVIGATION__LOOK_AT_HPP_ -#include +#include + #include +#include +#include "attention_system_msgs/msg/attention_command.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" -#include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" +#include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "attention_system_msgs/msg/attention_points.hpp" - #include "std_msgs/msg/string.hpp" -namespace navigation +namespace head { class LookAt : public BT::ActionNodeBase { public: - explicit LookAt( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf); + explicit LookAt(const std::string & xml_tag_name, const BT::NodeConfiguration & conf); void halt(); BT::NodeStatus tick(); @@ -44,21 +43,20 @@ class LookAt : public BT::ActionNodeBase static BT::PortsList providedPorts() { return BT::PortsList( - { - BT::InputPort>("tf_frames"), - BT::InputPort("tf_frame") - }); + {BT::InputPort>("tf_frames"), + BT::InputPort("tf_frame")}); } private: rclcpp::Node::SharedPtr node_; geometry_msgs::msg::PoseStamped pose_; - rclcpp::Publisher::SharedPtr attention_points_pub_; + rclcpp::Publisher::SharedPtr attention_points_pub_; rclcpp::Publisher::SharedPtr test_pub_; std::vector tf_frames_; std::string tf_frame_; + std::shared_ptr tf_buffer_; }; } // namespace navigation -#endif // NAVIGATION__LOOK_AT_HPP_ +#endif // NAVIGATION__LOOK_AT_HPP_ \ No newline at end of file diff --git a/bt_nodes/motion/include/motion/head/Pan.hpp b/bt_nodes/motion/include/motion/head/Pan.hpp new file mode 100644 index 0000000..00ef065 --- /dev/null +++ b/bt_nodes/motion/include/motion/head/Pan.hpp @@ -0,0 +1,60 @@ +// Copyright 2024 Intelligent Robotics Lab - Gentlebots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HEAD__PAN_HPP_ +#define HEAD__PAN_HPP_ + + +#include + +#include "behaviortree_cpp_v3/behavior_tree.h" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "ctrl_support/BTActionNode.hpp" +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace head +{ + +class Pan : public motion::BtActionNode +{ +public: + explicit Pan( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf); + + + void on_tick() override; + void on_feedback() override; + BT::NodeStatus on_success() override; + BT::NodeStatus on_aborted() override; + BT::NodeStatus on_cancelled() override; + + + static BT::PortsList providedPorts() + { + return BT::PortsList( + { + BT::InputPort("tf_frame"), + }); + } +private: + rclcpp::Node::SharedPtr node_; + BT::Optional point_to_pan_; +}; + +} // namespace receptionist + +#endif // RECEPTIONIST__BEHAVIOR_TREES_NODES__PAN_TO_POINT_HPP_ diff --git a/bt_nodes/motion/include/motion/navigation/MoveTo.hpp b/bt_nodes/motion/include/motion/navigation/MoveTo.hpp index d9107fe..877d1af 100644 --- a/bt_nodes/motion/include/motion/navigation/MoveTo.hpp +++ b/bt_nodes/motion/include/motion/navigation/MoveTo.hpp @@ -20,7 +20,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" -#include "motion/navigation/ctrl_support/BTActionNode.hpp" +#include "ctrl_support/BTActionNode.hpp" #include "behaviortree_cpp_v3/behavior_tree.h" #include "behaviortree_cpp_v3/bt_factory.h" @@ -37,7 +37,7 @@ namespace navigation { class MoveTo - : public navigation::BtActionNode + : public motion::BtActionNode { public: explicit MoveTo( diff --git a/bt_nodes/motion/src/motion/head/LookAt.cpp b/bt_nodes/motion/src/motion/head/LookAt.cpp index cf4fba1..623da85 100644 --- a/bt_nodes/motion/src/motion/head/LookAt.cpp +++ b/bt_nodes/motion/src/motion/head/LookAt.cpp @@ -12,30 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "motion/navigation/LookAt.hpp" +#include "motion/head/LookAt.hpp" -namespace navigation +namespace head { -LookAt::LookAt( - const std::string & xml_tag_name, - const BT::NodeConfiguration & conf) +LookAt::LookAt(const std::string & xml_tag_name, const BT::NodeConfiguration & conf) : BT::ActionNodeBase(xml_tag_name, conf) { config().blackboard->get("node", node_); rclcpp::QoS qos(rclcpp::KeepLast(10)); qos.transient_local().reliable(); - attention_points_pub_ = node_->create_publisher( - "/attention/attention_points", qos); + attention_points_pub_ = node_->create_publisher( + "attention/attention_command", 1); } -BT::NodeStatus -LookAt::tick() +BT::NodeStatus LookAt::tick() { - RCLCPP_INFO(node_->get_logger(), "LookAt ticked"); + RCLCPP_DEBUG(node_->get_logger(), "LookAt ticked"); getInput("tf_frames", tf_frames_); getInput("tf_frame", tf_frame_); + if (status() == BT::NodeStatus::IDLE) { + RCLCPP_DEBUG(node_->get_logger(), "IsPointing ticked"); + config().blackboard->get("tf_buffer", tf_buffer_); + } + std::string goal_frame; if (tf_frames_.size() == 0 && !tf_frame_.empty()) { @@ -44,40 +46,25 @@ LookAt::tick() goal_frame = tf_frames_[0]; } else { RCLCPP_ERROR(node_->get_logger(), "No goal frame provided"); - return BT::NodeStatus::FAILURE; + return BT::NodeStatus::RUNNING; } - attention_system_msgs::msg::AttentionPoints attention_points_msg; + attention_system_msgs::msg::AttentionCommand attention_command_msg; RCLCPP_INFO(node_->get_logger(), "LookAt tf_frame_: %s", goal_frame.c_str()); - attention_points_msg.instance_id = "look_at"; - attention_points_msg.lifeness = rclcpp::Duration(5, 0); - attention_points_msg.time_in_point = rclcpp::Duration(0, 0); + attention_command_msg.frame_id_to_track = goal_frame; - geometry_msgs::msg::PointStamped point; - point.header.frame_id = goal_frame; - point.point.x = 0.0; - point.point.y = 0.0; - point.point.z = 0.0; - - attention_points_msg.attention_points.push_back(point); - attention_points_pub_->publish(attention_points_msg); + attention_points_pub_->publish(attention_command_msg); rclcpp::spin_some(node_); RCLCPP_INFO(node_->get_logger(), "LookAt published attention points"); return BT::NodeStatus::SUCCESS; } -void -LookAt::halt() -{ - RCLCPP_INFO(node_->get_logger(), "LookAt halted"); -} - +void LookAt::halt() {RCLCPP_INFO(node_->get_logger(), "LookAt halted");} } // namespace navigation -BT_REGISTER_NODES(factory) -{ - factory.registerNodeType("LookAt"); -} +BT_REGISTER_NODES(factory) { + factory.registerNodeType("LookAt"); +} \ No newline at end of file diff --git a/bt_nodes/motion/src/motion/head/Pan.cpp b/bt_nodes/motion/src/motion/head/Pan.cpp new file mode 100644 index 0000000..092aefc --- /dev/null +++ b/bt_nodes/motion/src/motion/head/Pan.cpp @@ -0,0 +1,89 @@ +// Copyright 2024 Intelligent Robotics Lab - Gentlebots +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "motion/head/Pan.hpp" + +#include "behaviortree_cpp_v3/behavior_tree.h" + +namespace head +{ + +Pan::Pan( + const std::string & xml_tag_name, + const std::string & action_name, + const BT::NodeConfiguration & conf) +: motion::BtActionNode(xml_tag_name, action_name, conf) +{ + config().blackboard->get("node", node_); +} + + +void +Pan::on_tick() +{ + rclcpp::spin_some(node_); + BT::Optional frame_to_pan = getInput("tf_frame"); + + if (!frame_to_pan) { + RCLCPP_ERROR(node_->get_logger(), "Pan: tf_frame is missing"); + return; + } + + if (status() == BT::NodeStatus::IDLE) { + RCLCPP_INFO(node_->get_logger(), "Pan: tf_frame %s", frame_to_pan.value().c_str()); + goal_.trajectory.joint_names = std::vector{"head_1_joint", "head_2_joint"}; + // trajectory_msgs::msg::JointTrajectoryPoint point; + // point.positions = std::vector{point_to_pan_.value(), 0.0}; + // point.time_from_start = rclcpp::Duration::from_seconds(5.0); + + // goal_.trajectory.points.push_back(point); + + } +} + +void +Pan::on_feedback() +{ + RCLCPP_INFO(node_->get_logger(), "Pan: Feedback"); + control_msgs::action::FollowJointTrajectory::Feedback::SharedPtr feedback; + config().blackboard->get("feedback", feedback); + trajectory_msgs::msg::JointTrajectoryPoint actual = feedback->actual; + +} + +BT::NodeStatus +Pan::on_success() +{ + RCLCPP_INFO(node_->get_logger(), "Pan: Succeeded"); + return BT::NodeStatus::SUCCESS; +} + +} // namespace head + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + BT::NodeBuilder builder = [](const std::string & name, + const BT::NodeConfiguration & config) { + return std::make_unique( + name, "/head_controller/follow_joint_trajectory", config); + }; + + factory.registerBuilder( + "Pan", builder); + +} diff --git a/bt_nodes/motion/src/motion/navigation/MoveTo.cpp b/bt_nodes/motion/src/motion/navigation/MoveTo.cpp index 8320cda..474726c 100644 --- a/bt_nodes/motion/src/motion/navigation/MoveTo.cpp +++ b/bt_nodes/motion/src/motion/navigation/MoveTo.cpp @@ -32,7 +32,7 @@ MoveTo::MoveTo( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: navigation::BtActionNode(xml_tag_name, +: motion::BtActionNode(xml_tag_name, action_name, conf), tf_buffer_(), tf_listener_(tf_buffer_)