route_node.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 <functional>
#include <std_msgs/msg/string.hpp>
#include <carma_planning_msgs/msg/route.hpp>
#include <carma_planning_msgs/msg/route_state.hpp>
#include <carma_planning_msgs/msg/route_event.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <carma_planning_msgs/srv/get_available_routes.hpp>
#include <carma_planning_msgs/srv/set_active_route.hpp>
#include <carma_planning_msgs/srv/abort_active_route.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
 
#include <carma_wm/WMListener.hpp>
#include <carma_wm/WorldModel.hpp>
#include <tf2_ros/transform_listener.h>
 
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include "route/route_config.hpp"
#include "route/route_generator_worker.hpp"
 
namespace route
{
 
  class Route : public carma_ros2_utils::CarmaLifecycleNode
  {
 
  private:
    // Subscribers
    carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> twist_sub_;
    carma_ros2_utils::SubPtr<std_msgs::msg::String> geo_sub_;
 
    // Publishers
    carma_ros2_utils::PubPtr<carma_planning_msgs::msg::Route> route_pub_;
    carma_ros2_utils::PubPtr<carma_planning_msgs::msg::RouteState> route_state_pub_;
    carma_ros2_utils::PubPtr<carma_planning_msgs::msg::RouteEvent> route_event_pub_;
    carma_ros2_utils::PubPtr<visualization_msgs::msg::Marker> route_marker_pub_;
 
    // Service Servers
    carma_ros2_utils::ServicePtr<carma_planning_msgs::srv::GetAvailableRoutes> get_available_route_srv_;
    carma_ros2_utils::ServicePtr<carma_planning_msgs::srv::SetActiveRoute> set_active_route_srv_;
    carma_ros2_utils::ServicePtr<carma_planning_msgs::srv::AbortActiveRoute> abort_active_route_srv_;
 
    // Timers
    rclcpp::TimerBase::SharedPtr spin_timer_;
 
    // Node configuration
    Config config_;
 
    // tf2 buffer holds the tree of transforms
    tf2_ros::Buffer tf2_buffer_;
 
    // wm listener and pointer to the actual wm object
    carma_wm::WMListener wml_;
    carma_wm::WorldModelConstPtr wm_;
 
    // route generator worker
    RouteGeneratorWorker rg_worker_;
 
  public:
    explicit Route(const rclcpp::NodeOptions &);
 
    rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
 
    // Overrides
    carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &);
    carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &);
  };
 
} // route