Skip to content

Commit

Permalink
Improve descriptions
Browse files Browse the repository at this point in the history
  • Loading branch information
rafal-gorecki committed Oct 21, 2024
1 parent 433edce commit 73645dc
Show file tree
Hide file tree
Showing 11 changed files with 118 additions and 43 deletions.
57 changes: 57 additions & 0 deletions panther_manager/include/panther_manager/behavior_tree_utils.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,11 @@
#include "behaviortree_cpp/utils/shared_library.h"
#include "behaviortree_ros2/plugins.hpp"

#include <tf2/LinearMath/Quaternion.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/msg/pose_stamped.hpp>

namespace panther_manager::behavior_tree_utils
{

Expand Down Expand Up @@ -112,6 +117,58 @@ inline std::vector<float> convertFromString<std::vector<float>>(StringView str)
return output;
}

/**
* @brief Converts a string to a PoseStamped message.
*
* The string format should be "roll,pitch,yaw,x,y,z,frame_id" where:
* - roll, pitch, yaw: Euler angles in radians.
* - x, y, z: Position coordinates.
* - frame_id: Coordinate frame ID (string).
*
* @param str The string to convert.
* @return geometry_msgs::msg::PoseStamped The converted PoseStamped message.
*
* @throw BT::RuntimeError Throws if the input is invalid or cannot be parsed.
*/
template <>
inline geometry_msgs::msg::PoseStamped convertFromString<geometry_msgs::msg::PoseStamped>(
StringView str)
{
auto parts = splitString(str, ';');

if (parts.size() != 7) {
throw BT::RuntimeError(
"Invalid input for PoseStamped. Expected 7 values: roll;pitch;yaw;x;y;z;frame_id");
}

geometry_msgs::msg::PoseStamped pose_stamped;

try {
double roll = convertFromString<double>(parts[0]);
double pitch = convertFromString<double>(parts[1]);
double yaw = convertFromString<double>(parts[2]);

// Orientation form R,P,Y to Quaternion
tf2::Quaternion quaternion;
quaternion.setRPY(roll, pitch, yaw);
pose_stamped.pose.orientation = tf2::toMsg(quaternion);

// Position (x, y, z)
pose_stamped.pose.position.x = convertFromString<double>(parts[3]);
pose_stamped.pose.position.y = convertFromString<double>(parts[4]);
pose_stamped.pose.position.z = convertFromString<double>(parts[5]);

// Frame ID and current time
pose_stamped.header.frame_id = parts[6].to_string();
pose_stamped.header.stamp = rclcpp::Clock().now();

} catch (const std::exception & e) {
throw BT::RuntimeError("Failed to convert string to PoseStamped: " + std::string(e.what()));
}

return pose_stamped;
}

} // namespace BT

#endif // PANTHER_MANAGER_BEHAVIOR_TREE_UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,8 @@ class CallSetBoolService : public BT::RosServiceNode<std_srvs::srv::SetBool>

static BT::PortsList providedPorts()
{
return providedBasicPorts({BT::InputPort<bool>("data", "true / false value")});
return providedBasicPorts(
{BT::InputPort<bool>("data", "Boolean value to send with the service request.")});
}

virtual bool setRequest(typename Request::SharedPtr & request) override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,11 @@ class CallSetLedAnimationService : public BT::RosServiceNode<panther_msgs::srv::

static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<unsigned>("id", "animation ID"),
BT::InputPort<std::string>("param", "optional parameter"),
BT::InputPort<bool>("repeating", "indicates if animation should repeat"),
});
return providedBasicPorts(
{BT::InputPort<unsigned>("id", "Animation ID to trigger."),
BT::InputPort<std::string>("param", "Optional animation parameter."),
BT::InputPort<bool>(
"repeating", "Specifies whether the animation should repeated continuously.")});
}

virtual bool setRequest(typename Request::SharedPtr & request) override;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -48,20 +48,31 @@ class DockRobot : public BT::RosActionNode<opennav_docking_msgs::action::DockRob

static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<bool>("use_dock_id", true, "Whether to use the dock's ID or dock pose fields"),
BT::InputPort<std::string>("dock_id", "Dock ID or name to use"),
BT::InputPort<std::string>("dock_type", "The dock plugin type, if using dock pose"),
BT::InputPort<float>(
"max_staging_time", 1000.0, "Maximum time to navigate to the staging pose"),
BT::InputPort<bool>(
"navigate_to_staging_pose", true, "Whether to autonomously navigate to staging pose"),
return providedBasicPorts(
{BT::InputPort<bool>(
"use_dock_id", true, "Determines whether to use the dock's ID or dock pose fields."),
BT::InputPort<std::string>(
"dock_id",
"Specifies the dock's ID or name from the dock database (used if 'use_dock_id' is true)."),
BT::InputPort<geometry_msgs::msg::PoseStamped>(
"dock_pose",
"Specifies the dock's pose (used if 'use_dock_id' is false). Format: "
"\"roll,pitch,yaw,x,y,z,frame_id\""),
BT::InputPort<std::string>(
"dock_type",
"Defines the type of dock being used when docking via pose. Not needed if only one dock "
"type is available."),
BT::InputPort<float>(
"max_staging_time", 120.0,
"Maximum time allowed (in seconds) for navigating to the dock's staging pose."),
BT::InputPort<bool>(
"navigate_to_staging_pose", true,
"Specifies whether the robot should autonomously navigate to the staging pose."),

BT::OutputPort<DockRobotActionResult::_error_code_type>(
"error_code", "Contextual error code, if any"),
BT::OutputPort<DockRobotActionResult::_num_retries_type>(
"num_retries", "Number of retries attempted"),
});
BT::OutputPort<DockRobotActionResult::_error_code_type>(
"error_code", "Returns an error code indicating the reason for failure, if any."),
BT::OutputPort<DockRobotActionResult::_num_retries_type>(
"num_retries", "Reports the number of retry attempts made during the docking process.")});
}
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,10 +39,8 @@ class ShutdownHostsFromFile : public ShutdownHosts

static BT::PortsList providedPorts()
{
return {
BT::InputPort<std::string>(
"shutdown_hosts_file", "global path to YAML file with hosts to shutdown"),
};
return {BT::InputPort<std::string>(
"shutdown_hosts_file", "Absolute path to a YAML file listing the hosts to shut down.")};
}

private:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,14 +38,17 @@ class ShutdownSingleHost : public ShutdownHosts
static BT::PortsList providedPorts()
{
return {
BT::InputPort<std::string>("ip", "ip of the host to shutdown"),
BT::InputPort<std::string>("username", "user to log into while executing shutdown command"),
BT::InputPort<unsigned>("port", "SSH communication port"),
BT::InputPort<std::string>("command", "command to execute on shutdown"),
BT::InputPort<float>("timeout", "time in seconds to wait for host to shutdown"),
BT::InputPort<std::string>("ip", "IP address of the host to shut down."),
BT::InputPort<std::string>(
"username", "Username to use for logging in and executing the shutdown command."),
BT::InputPort<unsigned>("port", "SSH port used for communication (default is usually 22)."),
BT::InputPort<std::string>("command", "Shutdown command to execute on the remote host."),
BT::InputPort<float>(
"timeout", "Maximum time (in seconds) to wait for the host to shut down."),
BT::InputPort<bool>(
"ping_for_success", "ping host until it is not available or timeout is reached"),
};
"ping_for_success",
"Whether to continuously ping the host until it becomes unreachable or the timeout is "
"reached.")};
}

private:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ class SignalShutdown : public BT::SyncActionNode
static BT::PortsList providedPorts()
{
return {
BT::InputPort<std::string>("reason", "", "reason to shutdown robot"),
BT::InputPort<std::string>("reason", "", "Reason to shutdown robot."),
};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -49,14 +49,17 @@ class UndockRobot : public BT::RosActionNode<opennav_docking_msgs::action::Undoc

static BT::PortsList providedPorts()
{
return providedBasicPorts({
BT::InputPort<std::string>(
"dock_type", "The dock plugin type, if not previous instance used for docking"),
BT::InputPort<float>(
"max_undocking_time", 30.0, "Maximum time to get back to the staging pose"),
return providedBasicPorts(
{BT::InputPort<std::string>(
"dock_type",
"Specifies the dock plugin type to use for undocking. If empty, the previously used dock "
"type is assumed."),
BT::InputPort<float>(
"max_undocking_time", 30.0,
"Maximum allowable time (in seconds) to undock and return to the staging pose."),

BT::OutputPort<UndockRobotActionResult::_error_code_type>("error_code", "Error code"),
});
BT::OutputPort<UndockRobotActionResult::_error_code_type>(
"error_code", "Returns an error code if the undocking process fails.")});
}
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,7 +45,8 @@ class CheckBoolMsg : public BT::RosTopicSubNode<std_msgs::msg::Bool>

static BT::PortsList providedPorts()
{
return providedBasicPorts({BT::InputPort<bool>("data", "state of data to accept a condition")});
return providedBasicPorts(
{BT::InputPort<bool>("data", "Specifies the expected state of the data field.")});
}
};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,9 +45,9 @@ class CheckJoyMsg : public BT::RosTopicSubNode<sensor_msgs::msg::Joy>
static BT::PortsList providedPorts()
{
return providedBasicPorts(
{BT::InputPort<std::vector<float>>("axes", "state of axes to accept a condition"),
BT::InputPort<std::vector<int>>("buttons", "state of buttons to accept a condition"),
BT::InputPort<double>("timeout", 0.0, "max time delay to accept a condition")});
{BT::InputPort<std::vector<float>>("axes", "", "Specifies the expected state of the axes field. An empty string (\"\") means the value is ignored."),
BT::InputPort<std::vector<int>>("buttons", "", "Specifies the expected state of the buttons field. An empty string (\"\") means the value is ignored."),
BT::InputPort<double>("timeout", 0.0, "Maximum allowable time delay to accept the condition."});
}

private:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,8 @@ class TickAfterTimeout : public BT::DecoratorNode

static BT::PortsList providedPorts()
{
return {BT::InputPort<float>("timeout", "time in s to wait before ticking child again")};
return {BT::InputPort<float>(
"timeout", "Time in seconds to wait before ticking the child node again")};
}

private:
Expand Down

0 comments on commit 73645dc

Please sign in to comment.