How to build subscribers?

[Guelakais]

How to build subscribers?

[Guelakais]

I have solved it and would like to show you a quite complex subscriber in ros2 rust.

use std::{sync::{Arc,Mutex}, time::Duration,env,f64};
use anyhow::Result;
use rayon::prelude::*;
use nalgebra::{Vector3,Quaternion};
use lanelet_msgs::msg::{Lanelet};
use geometry_msgs::msg::{Point,Pose,PoseStamped};
use std_msgs::msg::Header;
use nav_msgs::msg::Path;

fn quaternions_from_points(point1: Point, point2: Point) -> geometry_msgs::msg::Quaternion {

    let point1_vec = Vector3::new(point1.x, point1.y, point1.z); 
    let point2_vec = Vector3::new(point2.x, point2.y, point2.z);
  
    let axis = point2_vec.cross(&point1_vec).normalize();
    let angle = point2_vec.angle(&point1_vec);
    
    let half_angle = angle / 2.0;
  
    let quaternion = Quaternion::new(
      half_angle.cos(), 
      axis[0] * half_angle.sin(),
      axis[1] * half_angle.sin(),
      axis[2] * half_angle.sin()
    );
  
    geometry_msgs::msg::Quaternion {
      w: quaternion.w,
      x: quaternion.i,
      y: quaternion.j,
      z: quaternion.k
    }
  
  }
fn middle_point(point1:Point,point2:Point) -> Point{
    Point{
        x:(point1.x+point2.x)/2.0,
        y:(point1.y+point2.y)/2.0,
        z:(point1.z+point2.z)/2.0
    }
}
struct TempValues{
    left_point:Vec<Point>,
    right_point:Vec<Point>,
    middle_point:Point,
    index:usize
}
struct PathPlanning {
    node: Arc<rclrs::Node>,
    _publisher: Arc<rclrs::Publisher<Path>>,
    _subscription:Arc<rclrs::Subscription<Lanelet>>,
    data: Arc<Mutex<Option<Lanelet>>>
}
impl PathPlanning{
    fn new(context: &rclrs::Context) -> Result<Self, rclrs::RclrsError>{
        let node = rclrs::create_node(context, "lanelet_path_conversion").unwrap();
        let data = Arc::new(Mutex::new(None));
        let data_cb = Arc::clone(&data);
        let _publisher = node
            .create_publisher("citicar_roadway_path", rclrs::QOS_PROFILE_DEFAULT)
            .unwrap();
        let _subscription = node.create_subscription(
            "target_pose",
            rclrs::QOS_PROFILE_DEFAULT,
            move |msg:Lanelet|{*data_cb.lock().unwrap() = Some(msg);},
            ).unwrap();
        Ok(Self { node, _publisher,_subscription,data })
    }
    fn generate_path(&self) -> Result<(), rclrs::RclrsError> {
        if let Some(s) = &*self.data.lock().unwrap(){
        let lanelet = s;
        let array_length=lanelet.left_boundary.line_string.len();
        let path = Path{
            header: Header{
                frame_id: "map".to_string(),
                stamp: self.node.get_clock().now().to_ros_msg().unwrap()
            },
            poses:(0..=array_length-1)
            .into_par_iter().
            map(|index|{TempValues{
                left_point:lanelet.left_boundary.line_string.clone(),
                right_point:lanelet.right_boundary.line_string.clone(),
                middle_point:middle_point(lanelet.left_boundary.line_string[index].clone(),lanelet.right_boundary.line_string[index].clone()),
                index
            }})
            .map(|temp_values|{
            PoseStamped{
                header:lanelet.meta_info.clone(),
                pose:Pose{
                    position: temp_values.middle_point.clone(),
                    orientation:quaternions_from_points(
                        match temp_values.index+1 < array_length {
                            true => middle_point(temp_values.left_point[temp_values.index+1].clone(),temp_values.right_point[temp_values.index+1].clone()),
                            _ => middle_point(
                                Point{
                                    x:temp_values.left_point[temp_values.index].x.clone()*2.0-temp_values.left_point[temp_values.index-1].x.clone(),
                                    y:temp_values.left_point[temp_values.index].y.clone()*2.0-temp_values.left_point[temp_values.index-1].y.clone(),
                                    z:temp_values.left_point[temp_values.index].z.clone()*2.0-temp_values.left_point[temp_values.index-1].z.clone(),
                                },
                                Point{
                                    x:temp_values.right_point[temp_values.index].x.clone()*2.0-temp_values.right_point[temp_values.index-1].x.clone(),
                                    y:temp_values.right_point[temp_values.index].y.clone()*2.0-temp_values.right_point[temp_values.index-1].y.clone(),
                                    z:temp_values.right_point[temp_values.index].z.clone()*2.0-temp_values.right_point[temp_values.index-1].z.clone(),
                                }
                            )
                        },
                        temp_values.middle_point.clone()
                    )
                }
            }
        }).collect::<Vec<PoseStamped>>(),
        };
        self._publisher.publish(path).unwrap();}
        Ok(())    
    }
}

fn main() -> Result<(), rclrs::RclrsError> {
    let context = rclrs::Context::new(env::args()).unwrap();
    let path_planning = Arc::new(PathPlanning::new(&context).unwrap());
    let publisher_other_thread = Arc::clone(&path_planning);
    std::thread::spawn(move || -> Result<(), rclrs::RclrsError> {
        loop {
            std::thread::sleep(Duration::from_millis(1));
            publisher_other_thread.generate_path()?;
        }
    });
    let node_ref = &path_planning.node;
    rclrs::spin(node_ref.clone())
}