trajectory_visualizer::TrajectoryVisualizer Class Reference

#include <trajectory_visualizer.hpp>

Inheritance diagram for trajectory_visualizer::TrajectoryVisualizer:

Collaboration diagram for trajectory_visualizer::TrajectoryVisualizer:

Public Member Functions
	TrajectoryVisualizer (const rclcpp::NodeOptions &)
	TrajectoryVisualizer constructor. More...
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

Private Member Functions
void	callbackPlanTrajectory (carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg)

Private Attributes
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::TrajectoryPlan >	traj_sub_
carma_ros2_utils::PubPtr< visualization_msgs::msg::MarkerArray >	traj_marker_pub_
Config	config_
double	max_speed_
size_t	prev_marker_list_size_ = 0

Detailed Description

Definition at line 35 of file trajectory_visualizer.hpp.

Constructor & Destructor Documentation

TrajectoryVisualizer()

trajectory_visualizer::TrajectoryVisualizer::TrajectoryVisualizer ( const rclcpp::NodeOptions &  options )

explicit

TrajectoryVisualizer constructor.

Definition at line 26 of file trajectory_visualizer.cpp.

      : carma_ros2_utils::CarmaLifecycleNode(options)
  {
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.max_speed = declare_parameter<double>("max_speed", config_.max_speed);
  }

References config_, and trajectory_visualizer::Config::max_speed.

Member Function Documentation

callbackPlanTrajectory()

void trajectory_visualizer::TrajectoryVisualizer::callbackPlanTrajectory ( carma_planning_msgs::msg::TrajectoryPlan::UniquePtr  msg )

private

Definition at line 56 of file trajectory_visualizer.cpp.

    {
        if (msg->trajectory_points.size() == 0)
        {
           RCLCPP_WARN_STREAM(this->get_logger(),"No trajectory point in plan_trajectory! Returning");
        }
        visualization_msgs::msg::MarkerArray tmp_marker_array;
        // display by markers the velocity between each trajectory point/target time.
        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = "map";
        marker.header.stamp =  this->now();
        marker.type = visualization_msgs::msg::Marker::ARROW;
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.ns = "trajectory_visualizer";
 
 
        marker.scale.x = 2;
        marker.scale.y = 2;
        marker.scale.z = 1;
        marker.frame_locked = true;
 
        size_t count = std::max(prev_marker_list_size_, msg->trajectory_points.size());
 
        for (size_t i = 1; i < count; i++)
        {
            marker.id = i;
 
            if (i >= msg->trajectory_points.size()) { // If we need to delete previous points
                marker.action = visualization_msgs::msg::Marker::DELETE;
                tmp_marker_array.markers.push_back(marker);
                continue;
            }
 
            double max_speed = config_.max_speed * MPH_TO_MS;
            double speed = max_speed;
 
            rclcpp::Time t2 = msg->trajectory_points[i].target_time;
            rclcpp::Time t1 = msg->trajectory_points[i - 1].target_time;
 
            if (validateTime(t2) && validateTime(t1) && t2 > t1 ) {
                rclcpp::Duration dt = t2 - t1;
 
                double dx = msg->trajectory_points[i].x - msg->trajectory_points[i-1].x;
                double dy = msg->trajectory_points[i].y - msg->trajectory_points[i-1].y;
                double dist = sqrt(dx * dx + dy * dy);
 
                speed = dist/ dt.seconds();
            }
 
 
 
            // map color to the scale of the speed
            // red being the highest, green being the lowest (0ms)
 
            RCLCPP_DEBUG_STREAM(this->get_logger(),"Speed:" << speed << "ms, max_speed:" << max_speed << "ms");
            if (speed > max_speed)
            {
                RCLCPP_DEBUG_STREAM(this->get_logger(),"Speed was big, so capped at " << max_speed << "ms");
                speed = max_speed;
            }
 
            marker.color.r = 0.0f;
            marker.color.g = 0.0f;
            marker.color.b = 0.0f;
            marker.color.a = 1.0f;
 
 
            double ratio = speed / max_speed;
            if (ratio >= 0.75f)
            {
                marker.color.r = 1.0f;
            }
            else if (ratio >= 0.5f)
            {
                marker.color.b = 1.0f;
            }
            else if (ratio >= 0.25)
            {
                marker.color.b = 1.0f;
                marker.color.g = 1.0f;
 
            }
            else if (ratio >= 0.0)
            {
                marker.color.g = 1.0f;
            }
 
            if (speed < 1.0) // Less than 2.5 mph is usually stopping
            {
                // Gray
                marker.color.r = 0.5f;
                marker.color.g = 0.5f;
                marker.color.b = 0.5f;
            }
 
            marker.points = {};
            geometry_msgs::msg::Point start;
            start.x = msg->trajectory_points[i-1].x;
            start.y = msg->trajectory_points[i-1].y;
 
            geometry_msgs::msg::Point end;
            end.x = msg->trajectory_points[i].x;
            end.y = msg->trajectory_points[i].y;
            marker.points.push_back(start);
            marker.points.push_back(end);
 
            tmp_marker_array.markers.push_back(marker);
        }
 
        prev_marker_list_size_ = msg->trajectory_points.size();
        traj_marker_pub_->publish(tmp_marker_array);
    }

References config_, process_bag::i, trajectory_visualizer::Config::max_speed, trajectory_visualizer::MPH_TO_MS, prev_marker_list_size_, traj_marker_pub_, and trajectory_visualizer::validateTime().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

handle_on_configure()

carma_ros2_utils::CallbackReturn trajectory_visualizer::TrajectoryVisualizer::handle_on_configure

(

const rclcpp_lifecycle::State &

)

Definition at line 36 of file trajectory_visualizer.cpp.

  {
    // Setup subscribers
    traj_sub_ = create_subscription<carma_planning_msgs::msg::TrajectoryPlan>("plan_trajectory", 50,std::bind(&TrajectoryVisualizer::callbackPlanTrajectory,this,std_ph::_1));
    // Setup publisher
    traj_marker_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>("trajectory_visualizer", 1);
 
    // Return success if everything initialized successfully
    return CallbackReturn::SUCCESS;
  }

References callbackPlanTrajectory(), traj_marker_pub_, and traj_sub_.

Here is the call graph for this function:

Member Data Documentation

config_

Config trajectory_visualizer::TrajectoryVisualizer::config_

private

Definition at line 46 of file trajectory_visualizer.hpp.

Referenced by TrajectoryVisualizer(), and callbackPlanTrajectory().

max_speed_

double trajectory_visualizer::TrajectoryVisualizer::max_speed_

private

Definition at line 52 of file trajectory_visualizer.hpp.

prev_marker_list_size_

size_t trajectory_visualizer::TrajectoryVisualizer::prev_marker_list_size_ = 0

private

Definition at line 54 of file trajectory_visualizer.hpp.

Referenced by callbackPlanTrajectory().

traj_marker_pub_

carma_ros2_utils::PubPtr<visualization_msgs::msg::MarkerArray> trajectory_visualizer::TrajectoryVisualizer::traj_marker_pub_

private

Definition at line 43 of file trajectory_visualizer.hpp.

Referenced by callbackPlanTrajectory(), and handle_on_configure().

traj_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> trajectory_visualizer::TrajectoryVisualizer::traj_sub_

private

Definition at line 40 of file trajectory_visualizer.hpp.

Referenced by handle_on_configure().

---

The documentation for this class was generated from the following files:

• trajectory_visualizer/include/trajectory_visualizer.hpp
• trajectory_visualizer/src/trajectory_visualizer.cpp