#ifndef
VISUALIZER_HPP
#define
VISUALIZER_HPP
#include "gcopter/trajectory.hpp"
#include "gcopter/quickhull.hpp"
#include "gcopter/geo_utils.hpp"
#include "tf/transform_datatypes.h"
#include <iostream>
#include <memory>
#include <chrono>
#include <cmath>
#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
//
Visualizer for the planner
class Visualizer
{
private:
// config contains the scale for some markers
ros::NodeHandle nh;
// These are publishers for path, waypoints on the trajectory,
// the entire trajectory, the mesh of free-space polytopes,
// the edge of free-space polytopes, and spheres for safety radius
ros::Publisher routePub;
ros::Publisher wayPointsPub;
ros::Publisher trajectoryPub;
ros::Publisher meshPub;
ros::Publisher edgePub;
ros::Publisher spherePub;
ros::Publisher startFinalNodePub;
public:
ros::Publisher speedPub;
ros::Publisher thrPub;
ros::Publisher tiltPub;
ros::Publisher bdrPub;
public:
Visualizer(ros::NodeHandle &nh_)
: nh(nh_)
{
routePub = nh.advertise<visualization_msgs::Marker>("/
visualizer/route", 10);
wayPointsPub = nh.advertise<visualization_msgs::Marker>("/
visualizer/waypoints", 10);
trajectoryPub = nh.advertise<visualization_msgs::Marker>("/
visualizer/trajectory", 10);
meshPub = nh.advertise<visualization_msgs::Marker>("/
visualizer/mesh", 1000);
edgePub = nh.advertise<visualization_msgs::Marker>("/
visualizer/edge", 1000);
spherePub = nh.advertise<visualization_msgs::Marker>("/
visualizer/spheres", 1000);
speedPub = nh.advertise<std_msgs::Float64>("/
visualizer/speed", 1000);
thrPub = nh.advertise<std_msgs::Float64>("/
visualizer/total_thrust", 1000);
tiltPub = nh.advertise<std_msgs::Float64>("/
visualizer/tilt_angle", 1000);
bdrPub = nh.advertise<std_msgs::Float64>("/
visualizer/body_rate", 1000);
startFinalNodePub = nh.advertise<visualization_msgs::MarkerArray>("/
visualizer/start_final_node", 10);
}
// Visualize the trajectory and its front-end path
template <int D, int Dim>
inline void visualize(const Trajectory<D, Dim> &traj,
const std::vector<Eigen::Vector3d> &route)
{
// std::cout<<"route: "<<route.size()<<std::endl;
// for(auto pt:route)
// std::cout<<"pt: "<<pt.transpose()<<std::endl;
visualization_msgs::Marker routeMarker, wayPointsMarker, trajMarker;
routeMarker.id = 0;
routeMarker.type = visualization_msgs::Marker::LINE_LIST;
routeMarker.header.stamp = ros::Time::now();
routeMarker.header.frame_id = "world";
routeMarker.pose.orientation.w = 1.00;
routeMarker.action = visualization_msgs::Marker::ADD;
routeMarker.ns = "route";
routeMarker.color.r = 1.00;
routeMarker.color.g = 0.00;
routeMarker.color.b = 0.00;
routeMarker.color.a = 1.00;
routeMarker.scale.x = 0.1;
wayPointsMarker = routeMarker;
wayPointsMarker.id = -wayPointsMarker.id - 1;
wayPointsMarker.type = visualization_msgs::Marker::SPHERE_LIST;
wayPointsMarker.ns = "waypoints";
wayPointsMarker.color.r = 1.00;
wayPointsMarker.color.g = 0.00;
wayPointsMarker.color.b = 0.00;
wayPointsMarker.scale.x = 0.35;
wayPointsMarker.scale.y = 0.35;
wayPointsMarker.scale.z = 0.35;
trajMarker = routeMarker;
trajMarker.header.frame_id = "world";
trajMarker.id = 0;
trajMarker.ns = "trajectory";
trajMarker.color.r = 0.00;
trajMarker.color.g = 0.50;
trajMarker.color.b = 1.00;
trajMarker.scale.x = 0.10;
trajMarker.scale.y = 0.10;
trajMarker.scale.z = 0.10;
if (route.size() > 0)
{
bool first = true;
Eigen::Vector3d last;
for (auto it : route)
{
if (first)
{
first = false;
last = it;
continue;
}
geometry_msgs::Point point;
point.x = last(0);
point.y = last(1);
point.z = last(2);
routeMarker.points.push_back(point);
point.x = it(0);
point.y = it(1);
point.z = it(2);
routeMarker.points.push_back(point);
last = it;
}
routePub.publish(routeMarker);
}
if (traj.getPieceNum() > 0)
{
Eigen::MatrixXd wps = traj.getPositions();
for (int i = 0; i < wps.cols(); i++)
{
geometry_msgs::Point point;
point.x = wps.col(i)(0);
point.y = wps.col(i)(1);
point.z = wps.col(i)(2);
wayPointsMarker.points.push_back(point);
}
wayPointsPub.publish(wayPointsMarker);
}
if (traj.getPieceNum() > 0)
{
double T = 0.01;
Eigen::Vector4d lastX = traj.getPos(0.0);
for (double t = T; t < traj.getTotalDuration(); t += T)
{
geometry_msgs::Point point;
Eigen::Vector4d X = traj.getPos(t);
point.x = lastX(0);
point.y = lastX(1);
point.z = lastX(2);
trajMarker.points.push_back(point);
point.x = X(0);
point.y = X(1);
point.z = X(2);
trajMarker.points.push_back(point);
lastX = X;
}
trajectoryPub.publish(trajMarker);
}
}
详细注释上述代码
Java字节码生成交互式类图
介绍一款免费工具，能够从Java字节码自动生成交互式的类图，适用于软件设计和架构分析。
扫一扫
596
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
