Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: BehaviorTree/BehaviorTree.ROS2
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: humble
Choose a base ref
...
head repository: RIF-Robotics/BehaviorTree.ROS2
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: humble
Choose a head ref
Can’t automatically merge. Don’t worry, you can still create the pull request.
  • 2 commits
  • 3 files changed
  • 1 contributor

Commits on Oct 24, 2024

  1. Verified

    This commit was signed with the committer’s verified signature.
    JounQin JounQin
    Copy the full SHA
    f473310 View commit details

Commits on Oct 25, 2024

  1. Copy the full SHA
    e72ceb5 View commit details
35 changes: 33 additions & 2 deletions behaviortree_ros2/include/behaviortree_ros2/bt_action_node.hpp
Original file line number Diff line number Diff line change
@@ -22,6 +22,7 @@
#include "behaviortree_cpp/action_node.h"
#include "behaviortree_cpp/bt_factory.h"
#include "rclcpp_action/rclcpp_action.hpp"
#include <service_msgs/msg/service_event_info.hpp>

#include "behaviortree_ros2/ros_node_params.hpp"

@@ -84,6 +85,13 @@ class RosActionNode : public BT::ActionNodeBase
using WrappedResult = typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult;
using Feedback = typename ActionT::Feedback;

// SendGoal Introspection
using SendGoalService = typename ActionT::Impl::SendGoalService;
using SendGoalServiceEvent = typename SendGoalService::Event;
using SendGoalServiceEventPublisher = typename rclcpp::Publisher<SendGoalServiceEvent>;
using SendGoalServiceEventPublisherPtr = std::shared_ptr<SendGoalServiceEventPublisher>;
using SendGoalServiceRequest = typename SendGoalService::Request;

/** To register this class into the factory, use:
*
* factory.registerNodeType<>(node_name, params);
@@ -172,10 +180,12 @@ class RosActionNode : public BT::ActionNodeBase
bool action_name_may_change_ = false;
const std::chrono::milliseconds server_timeout_;
const std::chrono::milliseconds wait_for_server_timeout_;
rcl_service_introspection_state_t send_goal_service_introspection_state_;

private:

ActionClientPtr action_client_;
SendGoalServiceEventPublisherPtr pub_send_goal_request_event_;
rclcpp::CallbackGroup::SharedPtr callback_group_;
rclcpp::executors::SingleThreadedExecutor callback_group_executor_;

@@ -201,7 +211,8 @@ template<class T> inline
BT::ActionNodeBase(instance_name, conf),
node_(params.nh),
server_timeout_(params.server_timeout),
wait_for_server_timeout_(params.wait_for_server_timeout)
wait_for_server_timeout_(params.wait_for_server_timeout),
send_goal_service_introspection_state_(params.send_goal_service_introspection_state)
{
// Three cases:
// - we use the default action_name in RosNodeParams when port is empty
@@ -262,12 +273,19 @@ template<class T> inline

prev_action_name_ = action_name;

if (send_goal_service_introspection_state_ == RCL_SERVICE_INTROSPECTION_METADATA or
send_goal_service_introspection_state_ == RCL_SERVICE_INTROSPECTION_CONTENTS) {
// Create the publisher for the send goal event
pub_send_goal_request_event_ = node_->create_publisher<SendGoalServiceEvent>(action_name + "/_action/send_goal/_service_event", 1);
}

bool found = action_client_->wait_for_action_server(wait_for_server_timeout_);
if(!found)
{
RCLCPP_ERROR(node_->get_logger(), "%s: Action server with name '%s' is not reachable.",
name().c_str(), prev_action_name_.c_str());
}

return found;
}

@@ -355,6 +373,20 @@ template<class T> inline
future_goal_handle_ = action_client_->async_send_goal( goal, goal_options );
time_goal_sent_ = node_->now();

if (pub_send_goal_request_event_) {
// Send the introspection event for send_goal
SendGoalServiceEvent event_msg;
event_msg.info.event_type = service_msgs::msg::ServiceEventInfo::REQUEST_SENT;
event_msg.info.stamp = time_goal_sent_;
//event_msg.info.client_gid; // Needed? Set in goal_response_callback?

SendGoalServiceRequest request_msg;
request_msg.goal = goal;

event_msg.set__request({request_msg});
pub_send_goal_request_event_->publish(event_msg);
}

return NodeStatus::RUNNING;
}

@@ -451,4 +483,3 @@ template<class T> inline


} // namespace BT

13 changes: 9 additions & 4 deletions behaviortree_ros2/include/behaviortree_ros2/bt_service_node.hpp
Original file line number Diff line number Diff line change
@@ -133,7 +133,7 @@ class RosServiceNode : public BT::ActionNodeBase
* It must return either SUCCESS or FAILURE.
*/
virtual BT::NodeStatus onFailure(ServiceNodeErrorCode /*error*/)
{
{
return NodeStatus::FAILURE;
}

@@ -144,6 +144,7 @@ class RosServiceNode : public BT::ActionNodeBase
bool service_name_may_change_ = false;
const std::chrono::milliseconds service_timeout_;
const std::chrono::milliseconds wait_for_service_timeout_;
rcl_service_introspection_state_t service_introspection_state_;

private:

@@ -172,7 +173,8 @@ template<class T> inline
BT::ActionNodeBase(instance_name, conf),
node_(params.nh),
service_timeout_(params.server_timeout),
wait_for_service_timeout_(params.wait_for_server_timeout)
wait_for_service_timeout_(params.wait_for_server_timeout),
service_introspection_state_(params.service_introspection_state)
{
// check port remapping
auto portIt = config().input_ports.find("service_name");
@@ -224,7 +226,11 @@ template<class T> inline

callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());
service_client_ = node_->create_client<T>(service_name, rmw_qos_profile_services_default, callback_group_);
service_client_ = node_->create_client<T>(service_name, rclcpp::ServicesQoS(), callback_group_);

service_client_->configure_introspection(
node_->get_clock(), rclcpp::SystemDefaultsQoS(), service_introspection_state_);

prev_service_name_ = service_name;

bool found = service_client_->wait_for_service(wait_for_service_timeout_);
@@ -335,4 +341,3 @@ template<class T> inline


} // namespace BT

Original file line number Diff line number Diff line change
@@ -40,6 +40,9 @@ struct RosNodeParams
std::chrono::milliseconds server_timeout = std::chrono::milliseconds(1000);
// timeout used when detecting the server the first time
std::chrono::milliseconds wait_for_server_timeout = std::chrono::milliseconds(500);

rcl_service_introspection_state_t service_introspection_state;
rcl_service_introspection_state_t send_goal_service_introspection_state;
};

}