70 lines
3.7 KiB
C++
70 lines
3.7 KiB
C++
#pragma once
|
|
|
|
#include "behaviortree_ros2/bt_action_node.hpp"
|
|
#include "mg_msgs/action/move_point.hpp"
|
|
|
|
namespace mg {
|
|
|
|
class MovePointNode : public BT::RosActionNode<mg_msgs::action::MovePoint> {
|
|
public:
|
|
MovePointNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
|
BT::RosActionNode<mg_msgs::action::MovePoint>(name, conf, params) { }
|
|
|
|
static BT::PortsList providedPorts() {
|
|
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
|
|
BT::InputPort<double>("y_goal"),
|
|
BT::InputPort<double>("tolerance"),
|
|
BT::InputPort<double>("max_wheel_speed"),
|
|
BT::InputPort<double>("max_angular"),
|
|
BT::InputPort<double>("max_vel"),
|
|
BT::InputPort<double>("pos_mult"),
|
|
BT::InputPort<double>("ornt_mult"),
|
|
BT::InputPort<double>("t_ornt_mult"),
|
|
BT::InputPort<double>("obst_mult_a"),
|
|
BT::InputPort<double>("obst_mult_b"),
|
|
BT::InputPort<double>("obst_mult_c"),
|
|
BT::InputPort<std::string>("IgnoreList", "", {}) });
|
|
}
|
|
|
|
bool setGoal(Goal& goal) override {
|
|
auto x_goal = getInput<double>("x_goal");
|
|
auto y_goal = getInput<double>("y_goal");
|
|
auto tolerance = getInput<double>("tolerance");
|
|
auto max_wheel_speed = getInput<double>("max_wheel_speed");
|
|
auto max_angular = getInput<double>("max_angular");
|
|
auto max_vel = getInput<double>("max_vel");
|
|
auto pos_mult = getInput<double>("pos_mult");
|
|
auto ornt_mult = getInput<double>("ornt_mult");
|
|
auto t_ornt_mult = getInput<double>("t_ornt_mult");
|
|
auto obst_mult_a = getInput<double>("obst_mult_a");
|
|
auto obst_mult_b = getInput<double>("obst_mult_b");
|
|
auto obst_mult_c = getInput<double>("obst_mult_c");
|
|
goal.x = x_goal.value();
|
|
goal.y = y_goal.value();
|
|
goal.tolerance = tolerance.value_or(goal.tolerance);
|
|
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
|
|
goal.max_angular = max_angular.value_or(goal.max_angular);
|
|
goal.max_vel = max_vel.value_or(goal.max_vel);
|
|
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
|
|
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
|
|
goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
|
|
goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
|
|
goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
|
|
goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
|
|
return true;
|
|
}
|
|
|
|
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
|
|
|
|
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
|
|
return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
|
|
BT::NodeStatus::FAILURE;
|
|
}
|
|
|
|
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
|
|
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
|
|
return BT::NodeStatus::FAILURE;
|
|
}
|
|
};
|
|
|
|
} |