diff --git a/src/bin/transform_point_cloud2.rs b/src/bin/transform_point_cloud2.rs index 50e5aad..de067fb 100644 --- a/src/bin/transform_point_cloud2.rs +++ b/src/bin/transform_point_cloud2.rs @@ -2,19 +2,17 @@ /// of the camera fov and the xy plane in the target frame, publish out as a marker and polygon /// based on camera_info_to_plane.py/.cpp and renamed to avoid rosrun confusion with the C++ node -// use nalgebra::{Point3, Rotation, Rotation3}; +use nalgebra::point; // , Rotation, Rotation3}; use roslibrust::ros1::{NodeHandle, Publisher}; use std::collections::HashMap; use tf_roslibrust::{ - // TfError, + TfError, TfListener, // tf_util, - // transforms::isometry_from_transform, + transforms::isometry_from_transform, }; use tokio::time::Duration; -use ros_pointcloud2::prelude::PointXYZ; - // this is already done in ros_pointcloud2, but that was fine in the case of tf_roslibrust and // tf_demo- is there something in the structure of ros_pointcloud2 that breaks this? roslibrust_codegen_macro::find_and_generate_ros_messages!(); @@ -155,6 +153,13 @@ async fn main() -> Result<(), anyhow::Error> { let nh = NodeHandle::new(&std::env::var("ROS_MASTER_URI")?, full_node_name) .await?; + let transformed_point_cloud_pub: Publisher = nh.advertise(&format!("{}/point_cloud_transformed_rust", ns.as_str()), 3).await?; + + // TODO(lucasw) don't need channels yet but later switch away from tokio::select and use + // channels between threads/tasks? + // let (pc_tc, pc_rx) = tokio::sync::oneshot::channel(); + let mut clouds_in = std::collections::VecDeque::new(); + // TODO(lucasw) remember leading ns or it won't work let mut point_cloud_sub = nh.subscribe::(&format!("{}/point_cloud", ns.as_str()), 10).await?; @@ -207,11 +212,8 @@ async fn main() -> Result<(), anyhow::Error> { // be generated twice like that? // the error mentions ros_pointcloud2::prelude::sensor_msgs; // let pc2_msg1: ros_pointcloud2::prelude::sensor_msgs::PointCloud2 = pc2_msg; - let pc: ros_pointcloud2::PointCloud2Msg = pc2_msg.into(); // .try_into_iter().unwrap(); - let points: Vec = pc.try_into_vec().unwrap(); - for pt in points { - println!("{pt:?}"); - } + // tx.send(pc); + clouds_in.push_back(pc2_msg); }, Some(Err(error)) => { println!("rx error: {error}"); @@ -220,6 +222,62 @@ async fn main() -> Result<(), anyhow::Error> { } } _ = update_interval.tick() => { + let cloud_in = clouds_in.pop_front(); + match cloud_in { + None => {}, + Some(cloud_in) => { + // TODO(lucasw) turn this into a function, return Some with enum that is the + // transformed cloud to be published, the unused cloud to be re-queued, + // an error (because there was a different error and don't + // try transforming the same cloud later) + let res = listener.lookup_transform( + &target_frame, + &cloud_in.header.frame_id, + None, + ); + match res { + Ok(tfs) => { + let stamp = cloud_in.header.stamp.clone(); + let cloud_in: ros_pointcloud2::PointCloud2Msg = cloud_in.into(); // .try_into_iter().unwrap(); + println!("{} to {}", cloud_in.header.frame_id, target_frame); + let cloud_to_target = isometry_from_transform(&tfs.transform); + // TODO(lucasw) once this is a function if this fails then + // return the error + let points_in: Vec = cloud_in.try_into_vec().unwrap(); + let mut points_out = Vec::new(); + for pt_in in points_in { // .iter().take(10) { + let pt_in = point![pt_in.x as f64, pt_in.y as f64, pt_in.z as f64]; + let pt_out = cloud_to_target * pt_in; + points_out.push(ros_pointcloud2::prelude::PointXYZ::new( + pt_out.x as f32, + pt_out.y as f32, + pt_out.z as f32, + )); + // println!("{pt:?}"); + } + let pc_out = ros_pointcloud2::PointCloud2Msg::try_from_vec(points_out).unwrap(); + let mut pc_out_msg: sensor_msgs::PointCloud2 = pc_out.into(); + pc_out_msg.header.stamp = stamp; + pc_out_msg.header.frame_id = target_frame.clone(); + transformed_point_cloud_pub.publish(&pc_out_msg).await?; + + // println!("{cloud_to_target:?}"); + println!("clouds left: {}", clouds_in.len()); + }, + Err(err) => match err { + TfError::AttemptedLookUpInFuture(_, _) => { + print!("-"); + // try to process again later + clouds_in.push_front(cloud_in); + }, + _ => { + // give up on this point cloud with any other error + println!("lookup err, discarding point cloud {err:?}"); + }, + }, + } + }, + } // handle input cloud }, // update } // tokio select loop }