Program Listing for File abstract_visualizer.hpp

↰ Return to documentation for file (include/bitbots_splines/abstract_visualizer.hpp)

#ifndef BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_VISUALIZER_H_
#define BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_VISUALIZER_H_

#include <tf2/LinearMath/Vector3.h>

#include <bitbots_splines/pose_spline.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

namespace bitbots_splines {

class AbstractVisualizer {
 protected:
  int id = 0;
  visualization_msgs::msg::Marker getMarker(const tf2::Vector3 &position, const std::string &frame,
                                            rclcpp::Node::SharedPtr node) {
    visualization_msgs::msg::Marker marker;

    marker.action = visualization_msgs::msg::Marker::ADD;
    marker.type = visualization_msgs::msg::Marker::SPHERE;
    marker.lifetime = rclcpp::Duration::from_nanoseconds(1e9 * 1000);
    marker.frame_locked = false;
    marker.header.frame_id = frame;
    marker.header.stamp = node->now();
    marker.pose.position.x = position.x();
    marker.pose.position.y = position.y();
    marker.pose.position.z = position.z();
    marker.pose.orientation.w = 1;
    marker.scale.x = 0.03;
    marker.scale.y = 0.03;
    marker.scale.z = 0.03;
    marker.color.a = 1;

    return marker;
  }

  visualization_msgs::msg::MarkerArray getPath(bitbots_splines::PoseSpline &spline, const std::string &frame,
                                               const double smoothness, rclcpp::Node::SharedPtr node) {
    visualization_msgs::msg::MarkerArray marker_array;
    visualization_msgs::msg::Marker base_marker;
    base_marker.action = visualization_msgs::msg::Marker::ADD;
    base_marker.lifetime = rclcpp::Duration::from_nanoseconds(1e9 * 1000);
    base_marker.frame_locked = true;
    base_marker.header.frame_id = frame;
    base_marker.header.stamp = node->now();
    base_marker.pose.orientation.w = 1;
    base_marker.color.a = 1;

    // Marker for spline path
    visualization_msgs::msg::Marker path_marker = base_marker;
    path_marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
    path_marker.id = id++;
    path_marker.scale.x = 0.01;

    // Take length of spline, assuming values at the beginning and end are set for x
    double first_time = spline.x()->min();
    double last_time = spline.x()->max();

    // Iterate over splines, get points everywhere
    // Taking the manually set points is not enough because velocities and accelerations influence the curve
    for (double i = first_time; i <= last_time; i += (last_time - first_time) / smoothness) {
      geometry_msgs::msg::Point point;
      point.x = spline.x()->pos(i);
      point.y = spline.y()->pos(i);
      point.z = spline.z()->pos(i);

      path_marker.points.push_back(point);
    }
    marker_array.markers.push_back(path_marker);

    // Marker for spline orientation
    // Get all times of manually added points
    std::list<double> times;
    for (const SmoothSpline::Point &p : spline.roll()->points()) {
      times.push_back(p.time);
    }
    for (const SmoothSpline::Point &p : spline.pitch()->points()) {
      times.push_back(p.time);
    }
    for (const SmoothSpline::Point &p : spline.yaw()->points()) {
      times.push_back(p.time);
    }
    times.sort();
    times.unique();

    visualization_msgs::msg::Marker orientation_marker = base_marker;
    orientation_marker.scale.x = 0.05;
    orientation_marker.scale.y = 0.005;
    orientation_marker.scale.z = 0.005;
    orientation_marker.type = visualization_msgs::msg::Marker::ARROW;
    for (double time : times) {
      orientation_marker.id = id++;
      orientation_marker.pose.position = spline.getGeometryMsgPosition(time);

      // We use three axes for every orientation
      // x
      orientation_marker.color.b = 0;
      orientation_marker.color.r = 1;
      tf2::Quaternion x_rotation = spline.getOrientation(time) * tf2::Vector3(1, 0, 0);
      orientation_marker.pose.orientation = tf2::toMsg(x_rotation.normalize());
      marker_array.markers.push_back(orientation_marker);

      // y
      orientation_marker.color.r = 0;
      orientation_marker.color.g = 1;
      tf2::Quaternion y_rotation = spline.getOrientation(time) * tf2::Vector3(1, 1, 0);
      orientation_marker.pose.orientation = tf2::toMsg(y_rotation.normalize());
      orientation_marker.id = id++;
      marker_array.markers.push_back(orientation_marker);

      // z
      orientation_marker.color.g = 0;
      orientation_marker.color.b = 1;
      tf2::Quaternion z_rotation = spline.getOrientation(time) * tf2::Vector3(1, 0, 1);
      orientation_marker.pose.orientation = tf2::toMsg(z_rotation.normalize());
      orientation_marker.id = id++;
      marker_array.markers.push_back(orientation_marker);
    }
    return marker_array;
  }
};
}  // namespace bitbots_splines

#endif  // BITBOTS_SPLINES_INCLUDE_BITBOTS_SPLINES_ABSTRACT_VISUALIZER_H_