| 97 | } |
| 98 | |
| 99 | void drawFOV(const vector<Eigen::Vector3d>& list1, const vector<Eigen::Vector3d>& list2) { |
| 100 | visualization_msgs::Marker mk; |
| 101 | mk.header.frame_id = "world"; |
| 102 | mk.header.stamp = ros::Time::now(); |
| 103 | mk.id = 0; |
| 104 | mk.ns = "current_pose"; |
| 105 | mk.type = visualization_msgs::Marker::LINE_LIST; |
| 106 | mk.pose.orientation.x = 0.0; |
| 107 | mk.pose.orientation.y = 0.0; |
| 108 | mk.pose.orientation.z = 0.0; |
| 109 | mk.pose.orientation.w = 1.0; |
| 110 | |
| 111 | // auto color = PlanningVisualization::getColor((drone_id_ - 1) / double(drone_num_)); |
| 112 | Eigen::Vector4d color(0, 0, 0, 1); |
| 113 | |
| 114 | mk.color.r = color(0); |
| 115 | mk.color.g = color(1); |
| 116 | mk.color.b = color(2); |
| 117 | mk.color.a = color(3); |
| 118 | mk.scale.x = 0.04; |
| 119 | mk.scale.y = 0.04; |
| 120 | mk.scale.z = 0.04; |
| 121 | |
| 122 | // Clean old marker |
| 123 | mk.action = visualization_msgs::Marker::DELETE; |
| 124 | cmd_vis_pub.publish(mk); |
| 125 | |
| 126 | if (list1.size() == 0) return; |
| 127 | |
| 128 | // Pub new marker |
| 129 | geometry_msgs::Point pt; |
| 130 | for (int i = 0; i < int(list1.size()); ++i) { |
| 131 | pt.x = list1[i](0); |
| 132 | pt.y = list1[i](1); |
| 133 | pt.z = list1[i](2); |
| 134 | mk.points.push_back(pt); |
| 135 | |
| 136 | pt.x = list2[i](0); |
| 137 | pt.y = list2[i](1); |
| 138 | pt.z = list2[i](2); |
| 139 | mk.points.push_back(pt); |
| 140 | } |
| 141 | mk.action = visualization_msgs::Marker::ADD; |
| 142 | cmd_vis_pub.publish(mk); |
| 143 | } |
| 144 | |
| 145 | void drawCmd(const Eigen::Vector3d& pos, const Eigen::Vector3d& vec, const int& id, |
| 146 | const Eigen::Vector4d& color) { |