| 28 | { |
| 29 | |
| 30 | Move::Move( |
| 31 | const std::string & xml_tag_name, |
| 32 | const std::string & action_name, |
| 33 | const BT::NodeConfig & conf) |
| 34 | : plansys2::BtActionNode<test_msgs::action::Fibonacci>(xml_tag_name, action_name, conf) |
| 35 | { |
| 36 | rclcpp_lifecycle::LifecycleNode::SharedPtr node; |
| 37 | if (!config().blackboard->get("node", node)) { |
| 38 | RCLCPP_ERROR(node_->get_logger(), "Failed to get 'node' from the blackboard"); |
| 39 | } |
| 40 | |
| 41 | node->declare_parameter<std::vector<std::string>>( |
| 42 | "waypoints", std::vector<std::string>({})); |
| 43 | if (node->has_parameter("waypoints")) { |
| 44 | std::vector<std::string> wp_names; |
| 45 | |
| 46 | node->get_parameter_or("waypoints", wp_names, {}); |
| 47 | |
| 48 | for (auto & wp : wp_names) { |
| 49 | node->declare_parameter<std::vector<double>>( |
| 50 | "waypoint_coords." + wp, std::vector<double>({})); |
| 51 | |
| 52 | std::vector<double> coords; |
| 53 | if (node->get_parameter_or("waypoint_coords." + wp, coords, {})) { |
| 54 | geometry_msgs::msg::Pose pose; |
| 55 | pose.position.x = coords[0]; |
| 56 | pose.position.y = coords[1]; |
| 57 | pose.orientation = tf2::toMsg(tf2::Quaternion({0.0, 0.0, 1.0}, coords[2])); |
| 58 | |
| 59 | waypoints_[wp] = pose; |
| 60 | } else { |
| 61 | std::cerr << "No coordinate configured for waypoint [" << wp << "]" << std::endl; |
| 62 | } |
| 63 | } |
| 64 | } |
| 65 | } |
| 66 | |
| 67 | BT::NodeStatus |
| 68 | Move::on_tick() |