向行为树中添加平滑路径该方法

流水心清

请教大家个问题，在ros2的导航配置文件中，如何对规划好的全局路径进行平滑优化呢？如何在BT行为树中添加smoother_path节点？

1540302317

@553416962 我也遇到这个问题了，不会搞，按理来说插件应该已经配置好了呀，行为树应该也启用了这个节点，因为已经有smoothed_plan话题了，不过里面没数据，启用了但是没调用。网上找到最相关的是这个https://blog.gitcode.com/a85f053c74129be6427b55accdb1d4bf.html，但是还是不知道在哪里改，哈哈哈哈

流水心清

@1540302317 我这样搞过，但是失败了，直接报错了。

1540302317

@553416962 我尝试了一下，发现可以，但是还是存在一些疑问，写成博客了，https://blog.csdn.net/qq_45253884/article/details/150211597?spm=1001.2014.3001.5501

流水心清

@1540302317 你好，我尝试了你的方法，确实可以调用成功。我之前尝试是没有在要修改的配置文件中添加<Sequence> 标签，导致文件配置失败。行为树这里我也不是很懂，照猫画虎，
我这里还有个邪修的方法，路径平滑是一个动作服务端，我就自己写了个动作客户端，在路径那里进行调用，达到优化的目的。

// 创建动作客户端
    client_ = rclcpp_action::create_client<SmoothPath>(node_, "smooth_path");
void sendGoal(const nav_msgs::msg::Path &input_path)
{
    RCLCPP_INFO(node_->get_logger(), "发送路径给平滑器...");

    auto goal = SmoothPath::Goal();
    goal.path = input_path;

    auto send_goal_options = rclcpp_action::Client<SmoothPath>::SendGoalOptions();
    send_goal_options.result_callback =
        [this](const GoalHandle::WrappedResult &result) {
            switch (result.code) {
                case rclcpp_action::ResultCode::SUCCEEDED:
                    RCLCPP_INFO(node_->get_logger(), "路径平滑成功！");
                    savePath(result.result->path, path_d_data_);
                    break;
                case rclcpp_action::ResultCode::ABORTED:
                    RCLCPP_ERROR(node_->get_logger(), "路径平滑被中止");
                    break;
                case rclcpp_action::ResultCode::CANCELED:
                    RCLCPP_WARN(node_->get_logger(), "路径平滑被取消");
                    break;
                default:
                    RCLCPP_ERROR(node_->get_logger(), "未知错误");
                    break;
            }
        };

    client_->async_send_goal(goal, send_goal_options);
}

期待继续交流！