void TrajectoryMapWriter::draw(MapWriterInterface *interface) { if(!initialized_) return; hector_nav_msgs::GetRobotTrajectory srv_path; if (!service_client_.call(srv_path)) { ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str()); return; } std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses); size_t size = traj_vector.size(); std::vector<Eigen::Vector2f> pointVec; pointVec.resize(size); for (size_t i = 0; i < size; ++i){ const geometry_msgs::PoseStamped& pose (traj_vector[i]); pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y); } if (size > 0){ //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z); Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f); interface->drawPath(startVec, pointVec); } }