control_plugin.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2022 LEIDOS.
 *
 * 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.
 */
 
#pragma once
 
#include <rclcpp/rclcpp.hpp>
#include <carma_planning_msgs/msg/trajectory_plan.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <autoware_msgs/msg/control_command_stamped.hpp>
 
#include "carma_guidance_plugins/plugin_base_node.hpp"
 
namespace carma_guidance_plugins
{
 
  class ControlPlugin : public PluginBaseNode
  {
 
  private:
    // Subscriptions
    carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped> current_pose_sub_;
    carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> current_velocity_sub_;
    carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> trajectory_plan_sub_;
 
    // Publishers
    carma_ros2_utils::PubPtr<autoware_msgs::msg::ControlCommandStamped> vehicle_cmd_pub_;
 
    // Timers
    rclcpp::TimerBase::SharedPtr command_timer_;
 
 
  protected:
 
    boost::optional<geometry_msgs::msg::PoseStamped> current_pose_;
 
    //  NOTE: Only the twist.linear.x and header are guaranteed to be populated
    boost::optional<geometry_msgs::msg::TwistStamped> current_twist_;
 
    boost::optional<carma_planning_msgs::msg::TrajectoryPlan> current_trajectory_;
 
    // These callbacks do direct assignment into their respective member variables
    void current_pose_callback(geometry_msgs::msg::PoseStamped::UniquePtr msg);
    void current_twist_callback(geometry_msgs::msg::TwistStamped::UniquePtr msg);
 
    virtual void current_trajectory_callback(carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg);
 
 
  public:
    explicit ControlPlugin(const rclcpp::NodeOptions &);
 
    virtual ~ControlPlugin() = default;
 
    virtual autoware_msgs::msg::ControlCommandStamped generate_command() = 0;
 
    // Overrides
 
    // Non-Final to allow extending plugins to provide more detailed capabilities
    std::string get_capability() override;
 
    // Final
    uint8_t get_type() override final;
 
    carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &) override final;
    carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &) override final;
    carma_ros2_utils::CallbackReturn handle_on_deactivate(const rclcpp_lifecycle::State &) override final;
    carma_ros2_utils::CallbackReturn handle_on_cleanup(const rclcpp_lifecycle::State &) override final;
    carma_ros2_utils::CallbackReturn handle_on_shutdown(const rclcpp_lifecycle::State &) override final;
    carma_ros2_utils::CallbackReturn handle_on_error(const rclcpp_lifecycle::State &, const std::string &exception_string) override final;
  };
 
} // carma_guidance_plugins