| 64 | } |
| 65 | |
| 66 | void drawCmd(const Eigen::Vector3d& pos, const Eigen::Vector3d& vec, const int& id, |
| 67 | const Eigen::Vector4d& color) { |
| 68 | visualization_msgs::Marker mk_state; |
| 69 | mk_state.header.frame_id = "world"; |
| 70 | mk_state.header.stamp = ros::Time::now(); |
| 71 | mk_state.id = id; |
| 72 | mk_state.type = visualization_msgs::Marker::ARROW; |
| 73 | mk_state.action = visualization_msgs::Marker::ADD; |
| 74 | |
| 75 | mk_state.pose.orientation.w = 1.0; |
| 76 | mk_state.scale.x = 0.1; |
| 77 | mk_state.scale.y = 0.2; |
| 78 | mk_state.scale.z = 0.3; |
| 79 | |
| 80 | geometry_msgs::Point pt; |
| 81 | pt.x = pos(0); |
| 82 | pt.y = pos(1); |
| 83 | pt.z = pos(2); |
| 84 | mk_state.points.push_back(pt); |
| 85 | |
| 86 | pt.x = pos(0) + vec(0); |
| 87 | pt.y = pos(1) + vec(1); |
| 88 | pt.z = pos(2) + vec(2); |
| 89 | mk_state.points.push_back(pt); |
| 90 | |
| 91 | mk_state.color.r = color(0); |
| 92 | mk_state.color.g = color(1); |
| 93 | mk_state.color.b = color(2); |
| 94 | mk_state.color.a = color(3); |
| 95 | |
| 96 | cmd_vis_pub.publish(mk_state); |
| 97 | } |
| 98 | |
| 99 | void pauseCallback(std_msgs::Empty msg) { |
| 100 | local_traj_->resetDuration(); |