Program Listing for File cl_autoware.hpp

↰ Return to documentation for file (/tmp/ws/src/SMACC2/smacc2_sm_reference_library/sm_autoware_avp/include/sm_autoware_avp/clients/autoware_client/cl_autoware.hpp)

// Copyright 2021 RobosoftAI Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*****************************************************************************************************************
 *
 *   Authors: Pablo Inigo Blasco, Brett Aldrich
 *
 ******************************************************************************************************************/
#pragma once

#include <smacc2/client_base_components/cp_topic_publisher.hpp>
#include <smacc2/client_base_components/cp_topic_subscriber.hpp>

//#include <autoware_auto_msgs//>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <smacc2/smacc_client.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logging.hpp>

namespace sm_autoware_avp
{
namespace clients
{

  using namespace std::chrono_literals;
template <typename TSource, typename TObject>
struct EvAutoLocalized : sc::event<EvAutoLocalized<TSource, TObject>>
{
  geometry_msgs::msg::PoseWithCovarianceStamped location;
};

template <typename TSource, typename TObject>
struct EvGoalReached : sc::event<EvGoalReached<TSource, TObject>>
{
};

class ClAutoware : public smacc2::ISmaccClient
{
public:
  smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseWithCovarianceStamped> * cppubLocation_;

  smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseStamped> * cpppubGoalPose_;

  std::optional<geometry_msgs::msg::PoseWithCovarianceStamped> lastPose_;
  std::optional<geometry_msgs::msg::PoseStamped> lastGoal_;

  double goalToleranceMeters_ = 3.5;  //m

  std::function<void(const geometry_msgs::msg::PoseWithCovarianceStamped &)> postEvAutoLocalized_;
  std::function<void()> postEvGoalReached_;

  template <typename TOrthogonal, typename TSourceObject>
  void onOrthogonalAllocation()
  {
    this->postEvAutoLocalized_ = [this](const geometry_msgs::msg::PoseWithCovarianceStamped & msg)
    {
      auto ev = new EvAutoLocalized<TSourceObject, TOrthogonal>();
      ev->location = msg;
      this->postEvent(ev);
    };

    this->postEvGoalReached_ = [this]()
    {
      this->postEvent<EvGoalReached<TSourceObject, TOrthogonal>>();
    };
  }

  void onInitialize() override
  {
    /*
      auto cppubLocation = client->createNamedComponent<
      smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseStamped>>(
      "initialPoseEstimation", "/localization/initialpose");

    auto cpppubGoalPose = client->createNamedComponent<
      smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseStamped>>(
      "goalPose", "planning/goal_pose");
    */

    cppubLocation_ =
      this->getComponent<smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseWithCovarianceStamped>>(
        "initialPoseEstimation");

    cpppubGoalPose_ =
      this->getComponent<smacc2::components::CpTopicPublisher<geometry_msgs::msg::PoseStamped>>(
        "goalPose");

    // auto subscriberNdtPose = client->createNamedComponent<
    //   smacc2::components::CpTopicSubscriber<geometry_msgs::msg::PoseWithCovarianceStamped>>(
    //   "ndtPose", "/localization/ndt_pose");

    auto subscriberNdtPose = this->getComponent<
      smacc2::components::CpTopicSubscriber<geometry_msgs::msg::PoseWithCovarianceStamped>>(
      "ndtPose");

    subscriberNdtPose->onMessageReceived(&ClAutoware::onNdtPoseReceived, this);
  }

  bool checkGoalReached()
  {
    if (lastGoal_ && lastPose_)
    {
      auto & pos = this->lastPose_->pose.pose.position;
      auto & goalpos = this->lastGoal_->pose.position;

      double dx = pos.x - goalpos.x;
      double dy = pos.y - goalpos.y;
      double dz = pos.z - goalpos.z;

      double dist = sqrt(dx * dx + dy * dy + dz * dz);

      RCLCPP_INFO_STREAM_THROTTLE(getLogger(), *(getNode()->get_clock()),2000,  "[" << getName() << "] goal linear error: " << dist);

      if (dist < goalToleranceMeters_)
      {
        return true;
      }
    }

    return false;
  }

  void onNdtPoseReceived(const geometry_msgs::msg::PoseWithCovarianceStamped & msg)
  {
    lastPose_ = msg;
    this->postEvAutoLocalized_(msg);

    if(checkGoalReached())
    {
      this->postEvGoalReached_();
    }
  }

  void publishGoalPose(const geometry_msgs::msg::PoseStamped & msg)
  {
    lastGoal_ = msg;
    this->cpppubGoalPose_->publish(msg);
  }

  void publishInitialPoseEstimation(const geometry_msgs::msg::PoseWithCovarianceStamped & msg)
  {
    this->cppubLocation_->publish(msg);
  }
};
}  // namespace clients
}  // namespace sm_autoware_avp