Skip to content

Commit

Permalink
Pan wip
Browse files Browse the repository at this point in the history
  • Loading branch information
@rodperex
rodperex committed Apr 22, 2024
1 parent 6c21552 commit 764d1e1
Show file tree
Hide file tree
Showing 9 changed files with 380 additions and 51 deletions.
175 changes: 175 additions & 0 deletions bt_nodes/bt_test/bt_xml/receptionist.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,175 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action ID="SetWaypoints" name="set waypoints from ros params"/>
<Action ID="InitReceptionist" n="{n}"/>
<Repeat num_cycles="2">
<Sequence>
<Action ID="MoveTo" distance_tolerance="0" tf_frame="entrance"/>
<Action ID="Speak" say_text="Hi, this is Gentlebot. I'm ready to receive a new guest"/>
<RetryUntilSuccessful num_attempts="-1">
<Fallback>
<Condition ID="IsDetected" confidence="0.6" entity="person" frames="" max_depth="2" max_entities="1" order="depth_nearest"/>
<Inverter>
<Action ID="Speak" say_text="I can't see you, can you please stand in front of me?"/>
</Inverter>
<Delay delay_msec="3000">
<AlwaysFailure/>
</Delay>
</Fallback>
</RetryUntilSuccessful>
<Action ID="StoreDetection" entity="person_0" id="{n}"/>
<Action ID="LookAt" tf_frame="person_0"/>
<RetryUntilSuccessful num_attempts="-1">
<SequenceStar>
<Sequence>
<Action ID="Speak" say_text="What is your name?"/>
<Action ID="Listen" listen_text="{understood}"/>
<Action ID="Query" Intention="name" intention_value="{name_value}" text="{understood}"/>
<Action ID="DialogConfirmation" prompt="Is your name {name_value}?"/>
</Sequence>
<Sequence>
<Action ID="Speak" say_text="{name_value}, what would you like to drink?"/>
<Action ID="Listen" listen_text="{understood}"/>
<Action ID="Query" Intention="drink" intention_value="{drink_value}" text="{understood}"/>
<Action ID="DialogConfirmation" prompt="Is {drink_value} your choice of preference?"/>
</Sequence>
</SequenceStar>
</RetryUntilSuccessful>
<Action ID="Speak" say_text="Please follow me, i will get you to the party"/>
<Action ID="MoveTo" distance_tolerance="0" tf_frame="party"/>
<ReactiveSequence>
<ReactiveFallback>
<Action ID="LookAround" direction="left"/>
<Condition ID="IsDetected" confidence="0.6" entity="person" frames="{frames}" max_depth="2" max_entities="1" order="depth_nearest"/>
</ReactiveFallback>
<Action ID="LookAt" tf_frame="{frames}"/>
<Action ID="Speak" say_text="Hi, this is {name_value}, and their favorite drink is {favourite_drink}"/>
</ReactiveSequence>
<ReactiveFallback>
<Condition ID="IsSittable" tf_frame="chair_frame"/>
<Action ID="LookAround" direction="left"/>
</ReactiveFallback>
<Action ID="PointAt" frame_to_point="chair_frame"/>
<Action ID="LookAt" tf_frame="chair_frame"/>
<Action ID="Speak" say_text="Please, sit over there"/>
<Action ID="MoveArmToPredefined" position="home"/>
<Action ID="AddGuestToCount" n="{n}"/>
</Sequence>
</Repeat>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="AddGuestToCount">
<inout_port name="n">Number of guests introduced</inout_port>
</Action>
<Action ID="CalculateGoal">
<inout_port name="goal"/>
</Action>
<Condition ID="CheckStop"/>
<Action ID="DetectEntity">
<input_port default="person" name="entity"/>
</Action>
<Action ID="DialogConfirmation">
<input_port name="prompt"/>
</Action>
<Action ID="Dialogue">
<input_port default="Hi welcome to the party" name="say_text"/>
</Action>
<Action ID="ExtractBagPosition">
<output_port name="position"/>
</Action>
<Action ID="FollowEntity">
<input_port default="person" name="entity_frame"/>
</Action>
<Action ID="GoBack">
<input_port default="0.2" name="velocity"/>
</Action>
<Condition ID="HasEntityStopped">
<input_port name="entity_frame"/>
<input_port name="tolerance"/>
</Condition>
<Action ID="InitReceptionist">
<output_port default="0" name="n">Number of guests introduced</output_port>
</Action>
<Condition ID="IsDetected">
<input_port default="0.6" name="confidence"/>
<input_port default="person" name="entity"/>
<output_port name="frames">array of frames</output_port>
<input_port default="2" name="max_depth">value in meters</input_port>
<input_port default="1" name="max_entities"/>
<input_port default="depth_nearest" name="order">[depth_nearest, left_to_right]^</input_port>
</Condition>
<Condition ID="IsEntityMoving">
<input_port default="0.5" name="distance_tolerance"/>
</Condition>
<Condition ID="IsPointing">
<output_port default="front" name="tf_target">the tf frame of the object to navigate</output_port>
</Condition>
<Condition ID="IsSittable">
<output_port name="tf_frame"/>
</Condition>
<Action ID="Listen">
<output_port name="listen_text">What the robot understood</output_port>
</Action>
<Action ID="LookAround">
<input_port name="direction"/>
</Action>
<Action ID="LookAt">
<input_port name="tf_frame"/>
</Action>
<Action ID="LookAtPerson"/>
<Action ID="MoveArmTo">
<input_port name="tf_frame"/>
<input_port name="tolerance"/>
</Action>
<Action ID="MoveArmToPredefined">
<input_port default="offer" name="position"/>
</Action>
<Action ID="MoveGripperTo">
<input_port default="open" name="Pos"/>
</Action>
<Action ID="MoveHeadTo">
<input_port name="angle"/>
</Action>
<Action ID="MoveTo">
<input_port default="0" name="distance_tolerance"/>
<input_port default="object1" name="tf_frame"/>
</Action>
<Action ID="PointAt">
<input_port name="frame_to_point"/>
</Action>
<Action ID="PrintTarget">
<inout_port name="target"/>
</Action>
<Action ID="Query">
<input_port default="name" name="Intention"/>
<output_port name="intention_value"/>
<input_port name="text">what human said</input_port>
</Action>
<Action ID="SetWaypoints"/>
<Action ID="Speak">
<input_port name="say_text"/>
</Action>
<Action ID="StoreDetection">
<input_port name="entity"/>
<input_port name="id"/>
</Action>
<Condition ID="isListened">
<output_port name="gotten_parameters"/>
<input_port name="needed_parameters"/>
<output_port name="say_text"/>
</Condition>
<Condition ID="isPersonDetected">
<output_port name="say_text"/>
</Condition>
<Condition ID="isReceived">
<output_port name="needed_parameters"/>
<input_port name="received_parameters"/>
</Condition>
</TreeNodesModel>
<!-- ////////// -->
</root>
3 changes: 3 additions & 0 deletions bt_nodes/motion/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
19 changes: 18 additions & 1 deletion .../navigation/ctrl_support/BTActionNode.hpp → ...ion/include/ctrl_support/BTActionNode.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

namespace navigation
namespace motion
{

using namespace std::chrono_literals; // NOLINT
Expand Down Expand Up @@ -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(
Expand All @@ -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<ActionT>::SharedPtr,
const typename ActionT::Feedback::ConstSharedPtr feedback)
{
config().blackboard->set("feedback", feedback);
on_feedback();
}

virtual void on_feedback()
{

}

void increment_recovery_count()
{
Expand Down
26 changes: 12 additions & 14 deletions ...tion/include/motion/navigation/LookAt.hpp → ...des/motion/include/motion/head/LookAt.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,50 +15,48 @@
#ifndef NAVIGATION__LOOK_AT_HPP_
#define NAVIGATION__LOOK_AT_HPP_

#include <string>
#include <tf2_ros/buffer.h>

#include <memory>
#include <string>

#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();

static BT::PortsList providedPorts()
{
return BT::PortsList(
{
BT::InputPort<std::vector<std::string>>("tf_frames"),
BT::InputPort<std::string>("tf_frame")
});
{BT::InputPort<std::vector<std::string>>("tf_frames"),
BT::InputPort<std::string>("tf_frame")});
}

private:
rclcpp::Node::SharedPtr node_;
geometry_msgs::msg::PoseStamped pose_;
rclcpp::Publisher<attention_system_msgs::msg::AttentionPoints>::SharedPtr attention_points_pub_;
rclcpp::Publisher<attention_system_msgs::msg::AttentionCommand>::SharedPtr attention_points_pub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr test_pub_;
std::vector<std::string> tf_frames_;
std::string tf_frame_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
};

} // namespace navigation

#endif // NAVIGATION__LOOK_AT_HPP_
#endif // NAVIGATION__LOOK_AT_HPP_
60 changes: 60 additions & 0 deletions bt_nodes/motion/include/motion/head/Pan.hpp
View file
Original file line number Diff line number Diff line change
@@ -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 <string>

#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<control_msgs::action::FollowJointTrajectory>
{
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<std::string>("tf_frame"),
});
}
private:
rclcpp::Node::SharedPtr node_;
BT::Optional<std::string> point_to_pan_;
};

} // namespace receptionist

#endif // RECEPTIONIST__BEHAVIOR_TREES_NODES__PAN_TO_POINT_HPP_
4 changes: 2 additions & 2 deletions bt_nodes/motion/include/motion/navigation/MoveTo.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand All @@ -37,7 +37,7 @@ namespace navigation
{

class MoveTo
: public navigation::BtActionNode<nav2_msgs::action::NavigateToPose>
: public motion::BtActionNode<nav2_msgs::action::NavigateToPose>
{
public:
explicit MoveTo(
Expand Down
Loading

0 comments on commit 764d1e1

Please sign in to comment.