| 400 | } |
| 401 | |
| 402 | PoseStamped globalToLocal(double lat, double lon) |
| 403 | { |
| 404 | auto earth = GeographicLib::Geodesic::WGS84(); |
| 405 | |
| 406 | // Determine azimuth and distance between current and destination point |
| 407 | double _, distance, azimuth; |
| 408 | earth.Inverse(global_position.latitude, global_position.longitude, lat, lon, distance, _, azimuth); |
| 409 | |
| 410 | double x_offset, y_offset; |
| 411 | double azimuth_radians = azimuth * M_PI / 180; |
| 412 | x_offset = distance * sin(azimuth_radians); |
| 413 | y_offset = distance * cos(azimuth_radians); |
| 414 | |
| 415 | if (!waitTransform(local_frame, fcu_frame, global_position.header.stamp, ros::Duration(0.2))) { |
| 416 | throw std::runtime_error("No local position"); |
| 417 | } |
| 418 | |
| 419 | auto local = tf_buffer.lookupTransform(local_frame, fcu_frame, global_position.header.stamp); |
| 420 | |
| 421 | PoseStamped pose; |
| 422 | pose.header.stamp = global_position.header.stamp; // TODO: ? |
| 423 | pose.header.frame_id = local_frame; |
| 424 | pose.pose.position.x = local.transform.translation.x + x_offset; |
| 425 | pose.pose.position.y = local.transform.translation.y + y_offset; |
| 426 | pose.pose.orientation.w = 1; |
| 427 | return pose; |
| 428 | } |
| 429 | |
| 430 | // publish navigate_target frame |
| 431 | void publishTarget(ros::Time stamp, bool _static = false) |
no test coverage detected