From cd2e130c924c78a33852ac67c5a7e099bb824cfe Mon Sep 17 00:00:00 2001 From: ChenYing Kuo Date: Fri, 29 Nov 2024 14:05:26 +0800 Subject: [PATCH 1/2] Update README. Signed-off-by: ChenYing Kuo --- README.md | 42 ++---------------------------------------- 1 file changed, 2 insertions(+), 40 deletions(-) diff --git a/README.md b/README.md index 8a7dc2e..9d2a37f 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ # zenoh_carla_bridge The repository can bridge Carla and Autoware with Zenoh. -The bridge tranforms Carla data into Autoware ROS 2 topic and then sends it with Zenoh. +The bridge tranforms Carla data into Autoware ROS 2 message and then sends it via Zenoh. -If you want to have a quick demo, refer to [autoware_carla_launch](https://github.com/evshary/autoware_carla_launch) +If you want to have a quick demo, refer to [autoware_carla_launch](https://autoware-carla-launch.readthedocs.io/en/latest/) ## Build @@ -45,13 +45,6 @@ source /path/to/autoware/install/setup.bash RUST_LOG=c=info cargo run ``` -* Terminal5: Run ROS package to control vehicle - -```shell -# For example -ros2 run autoware_manual_control keyboard_control -``` - ## Note If you want to build in Ubuntu 22.04, remember to switch compiler's version. @@ -64,37 +57,6 @@ export LIBCLANG_STATIC_PATH=/usr/lib/llvm-12/lib export CLANG_PATH=/usr/bin/clang-12 ``` -## ROS topic - -### Input topic - -#### Control - -* `/control/command/control_cmd` -* `/control/command/emergency_cmd` -* `/control/command/gear_cmd` -* `/control/command/hazard_lights_cmd`: Not support -* `/control/command/turn_indicators_cmd`: Not support - -#### Sensing - -* `/sensing/camera/traffic_light/camera_info` -* `/sensing/camera/traffic_light/image_raw` -* `/sensing/gnss/pose`: Not support -* `/sensing/gnss/pose_with_covariance`: Not support -* `/sensing/imu/tamagawa/imu_raw` -* `/sensing/lidar/top/pointcloud_raw` -* `/sensing/lidar/top/pointcloud_raw_ex` - -### Output topic - -* `/vehicle/status/control_mode` -* `/vehicle/status/gear_status` -* `/vehicle/status/steering_status` -* `/vehicle/status/velocity_status` -* `/vehicle/status/hazard_lights_status`: Not support -* `/vehicle/status/turn_indicators_status`: Not support - ## For Developers You can use pre-commit and Ruff to have correct Python format From 3361aaf38f3ddb51897741d7025552bd4b0262a7 Mon Sep 17 00:00:00 2001 From: ChenYing Kuo Date: Fri, 29 Nov 2024 14:19:33 +0800 Subject: [PATCH 2/2] Fix the clippy issue. Signed-off-by: ChenYing Kuo --- .github/workflows/ci.yaml | 8 +++---- src/autoware.rs | 10 ++++---- src/bridge/actor_bridge.rs | 16 ++++++------- src/bridge/sensor_bridge.rs | 46 ++++++++++++++++-------------------- src/bridge/vehicle_bridge.rs | 14 +++++------ src/clock.rs | 8 ++++--- src/main.rs | 9 +++---- src/types.rs | 16 ++++++------- src/utils.rs | 2 +- 9 files changed, 64 insertions(+), 65 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 02ca36b..4de774a 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -49,10 +49,10 @@ jobs: - name: Run cargo check run: | cargo check --all --tests - #- name: Run cargo clippy - # run: | - # cargo clippy --version - # cargo clippy --all-targets -- -W warnings -D warnings + - name: Run cargo clippy + run: | + cargo clippy --version + cargo clippy --all-targets -- -W warnings -D warnings - name: Create Documentation run: | cargo doc --no-deps --all-features diff --git a/src/autoware.rs b/src/autoware.rs index 701b588..ee1409a 100644 --- a/src/autoware.rs +++ b/src/autoware.rs @@ -110,7 +110,7 @@ impl Autoware { SensorType::CameraRgb => { let raw_key = self.list_image_raw.get(sensor_name); let info_key = self.list_camera_info.get(sensor_name); - if raw_key != None && info_key != None { + if raw_key.is_some() && info_key.is_some() { return Some(vec![ raw_key.unwrap().to_owned(), info_key.unwrap().to_owned(), @@ -120,25 +120,25 @@ impl Autoware { SensorType::Collision => {} SensorType::Imu => { let imu_key = self.list_imu.get(sensor_name); - if imu_key != None { + if imu_key.is_some() { return Some(vec![imu_key.unwrap().to_owned()]); } } SensorType::LidarRayCast => { let lidar_key = self.list_lidar.get(sensor_name); - if lidar_key != None { + if lidar_key.is_some() { return Some(vec![lidar_key.unwrap().to_owned()]); } } SensorType::LidarRayCastSemantic => { let lidar_key = self.list_lidar_semantics.get(sensor_name); - if lidar_key != None { + if lidar_key.is_some() { return Some(vec![lidar_key.unwrap().to_owned()]); } } SensorType::Gnss => { let gnss_key = self.list_gnss.get(sensor_name); - if gnss_key != None { + if gnss_key.is_some() { return Some(vec![gnss_key.unwrap().to_owned()]); } } diff --git a/src/bridge/actor_bridge.rs b/src/bridge/actor_bridge.rs index b44040d..28ac941 100644 --- a/src/bridge/actor_bridge.rs +++ b/src/bridge/actor_bridge.rs @@ -12,11 +12,11 @@ use zenoh::Session; #[derive(Debug)] pub enum BridgeType { - BridgeTypeVehicle(String), - BridgeTypeSensor(String, SensorType, String), - BridgeTypeTrafficLight, - BridgeTypeTrafficSign, - BridgeTypeOther, + Vehicle(String), + Sensor(String, SensorType, String), + TrafficLight, + TrafficSign, + Other, } pub trait ActorBridge { @@ -27,9 +27,9 @@ pub fn get_bridge_type(actor: Actor) -> Result { Ok(match actor.into_kinds() { ActorKind::Vehicle(vehicle) => VehicleBridge::get_bridge_type(vehicle)?, ActorKind::Sensor(sensor) => SensorBridge::get_bridge_type(sensor)?, - ActorKind::TrafficLight(_) => BridgeType::BridgeTypeTrafficLight, - ActorKind::TrafficSign(_) => BridgeType::BridgeTypeTrafficSign, - ActorKind::Other(_) => BridgeType::BridgeTypeOther, + ActorKind::TrafficLight(_) => BridgeType::TrafficLight, + ActorKind::TrafficSign(_) => BridgeType::TrafficSign, + ActorKind::Other(_) => BridgeType::Other, }) } diff --git a/src/bridge/sensor_bridge.rs b/src/bridge/sensor_bridge.rs index 88b28d1..4839a04 100644 --- a/src/bridge/sensor_bridge.rs +++ b/src/bridge/sensor_bridge.rs @@ -104,11 +104,7 @@ impl SensorBridge { let sensor_type: SensorType = sensor_type_id.parse().or(Err(BridgeError::CarlaIssue( "Unable to recognize sensor type", )))?; - Ok(BridgeType::BridgeTypeSensor( - vehicle_name, - sensor_type, - sensor_name, - )) + Ok(BridgeType::Sensor(vehicle_name, sensor_type, sensor_name)) } pub fn new( z_session: Arc, @@ -117,7 +113,7 @@ impl SensorBridge { autoware: &Autoware, ) -> Result { let (vehicle_name, sensor_type, sensor_name) = match bridge_type { - BridgeType::BridgeTypeSensor(v, t, s) => (v, t, s), + BridgeType::Sensor(v, t, s) => (v, t, s), _ => panic!("Should never happen!"), }; @@ -180,13 +176,13 @@ fn register_camera_rgb( thread::spawn(move || loop { match rx.recv() { Ok((MessageType::SensorData, sensor_data)) => { - if let Err(_) = image_publisher.put(sensor_data).wait() { - log::error!("Failed to publish to {}", raw_key); + if let Err(e) = image_publisher.put(sensor_data).wait() { + log::error!("Failed to publish to {}: {:?}", raw_key, e); } } Ok((MessageType::InfoData, info_data)) => { - if let Err(_) = info_publisher.put(info_data).wait() { - log::error!("Failed to publish to {}", info_key); + if let Err(e) = info_publisher.put(info_data).wait() { + log::error!("Failed to publish to {}: {:?}", info_key, e); } } _ => { @@ -255,8 +251,8 @@ fn register_lidar_raycast( thread::spawn(move || loop { match rx.recv() { Ok((MessageType::SensorData, sensor_data)) => { - if let Err(_) = pcd_publisher.put(sensor_data).wait() { - log::error!("Failed to publish to {}", key); + if let Err(e) = pcd_publisher.put(sensor_data).wait() { + log::error!("Failed to publish to {}: {:?}", key, e); } } _ => { @@ -294,8 +290,8 @@ fn register_lidar_raycast_semantic( thread::spawn(move || loop { match rx.recv() { Ok((MessageType::SensorData, sensor_data)) => { - if let Err(_) = pcd_publisher.put(sensor_data).wait() { - log::error!("Failed to publish to {}", key); + if let Err(e) = pcd_publisher.put(sensor_data).wait() { + log::error!("Failed to publish to {}: {:?}", key, e); } } _ => { @@ -333,8 +329,8 @@ fn register_imu( thread::spawn(move || loop { match rx.recv() { Ok((MessageType::SensorData, sensor_data)) => { - if let Err(_) = imu_publisher.put(sensor_data).wait() { - log::error!("Failed to publish to {}", key); + if let Err(e) = imu_publisher.put(sensor_data).wait() { + log::error!("Failed to publish to {}: {:?}", key, e); } } _ => { @@ -371,8 +367,8 @@ fn register_gnss( thread::spawn(move || loop { match rx.recv() { Ok((MessageType::SensorData, sensor_data)) => { - if let Err(_) = gnss_publisher.put(sensor_data).wait() { - log::error!("Failed to publish to {}", key); + if let Err(e) = gnss_publisher.put(sensor_data).wait() { + log::error!("Failed to publish to {}: {:?}", key, e); } } _ => { @@ -684,11 +680,11 @@ fn gnss_callback( longitude: measure.longitude(), altitude: measure.attitude() + 17.0, status: sensor_msgs::NavSatStatus { - status: GnssStatus::StatusSbasFix as i8, - service: GnssService::ServiceGps as u16 - | GnssService::ServiceGlonass as u16 - | GnssService::ServiceCompass as u16 - | GnssService::ServiceGalileo as u16, + status: GnssStatus::SbasFix as i8, + service: GnssService::Gps as u16 + | GnssService::Glonass as u16 + | GnssService::Compass as u16 + | GnssService::Galileo as u16, }, position_covariance: [0.0; 9], position_covariance_type: 0, // unknown type @@ -710,8 +706,8 @@ impl Drop for SensorBridge { if self.sensor_type != SensorType::Collision && self.sensor_type != SensorType::NotSupport { // Not sure why the tx doesn't release in sensor callback, so rx can't use RecvErr to close the thread // I create another message type to notify the thread to close - if let Err(_) = self.tx.send((MessageType::StopThread, vec![])) { - log::error!("Unable to stop the thread") + if let Err(e) = self.tx.send((MessageType::StopThread, vec![])) { + log::error!("Unable to stop the thread: {:?}", e); } } } diff --git a/src/bridge/vehicle_bridge.rs b/src/bridge/vehicle_bridge.rs index 72ea052..35368a4 100644 --- a/src/bridge/vehicle_bridge.rs +++ b/src/bridge/vehicle_bridge.rs @@ -66,7 +66,7 @@ impl<'a> VehicleBridge<'a> { log::info!("Detect a vehicle {vehicle_name}"); - Ok(BridgeType::BridgeTypeVehicle(vehicle_name)) + Ok(BridgeType::Vehicle(vehicle_name)) } pub fn new( @@ -76,7 +76,7 @@ impl<'a> VehicleBridge<'a> { autoware: &Autoware, ) -> Result> { let vehicle_name = match bridge_type { - BridgeType::BridgeTypeVehicle(v) => v, + BridgeType::Vehicle(v) => v, _ => panic!("Should never happen!"), }; @@ -269,7 +269,7 @@ impl<'a> VehicleBridge<'a> { let control_msg = ControlModeReport { stamp: Time { sec: timestamp.floor() as i32, - nanosec: (timestamp.fract() * 1000_000_000_f64) as u32, + nanosec: (timestamp.fract() * 1_000_000_000_f64) as u32, }, mode, }; @@ -283,7 +283,7 @@ impl<'a> VehicleBridge<'a> { let turnindicator_msg = TurnIndicatorsReport { stamp: Time { sec: timestamp.floor() as i32, - nanosec: (timestamp.fract() * 1000_000_000_f64) as u32, + nanosec: (timestamp.fract() * 1_000_000_000_f64) as u32, }, report: turn_indicators_report::DISABLE, }; @@ -297,7 +297,7 @@ impl<'a> VehicleBridge<'a> { let hazardlight_msg = HazardLightsReport { stamp: Time { sec: timestamp.floor() as i32, - nanosec: (timestamp.fract() * 1000_000_000_f64) as u32, + nanosec: (timestamp.fract() * 1_000_000_000_f64) as u32, }, report: hazard_lights_report::DISABLE, }; @@ -368,7 +368,7 @@ impl<'a> VehicleBridge<'a> { } } -impl<'a> ActorBridge for VehicleBridge<'a> { +impl ActorBridge for VehicleBridge<'_> { fn step(&mut self, timestamp: f64) -> Result<()> { self.pub_current_velocity(timestamp)?; self.pub_current_steer(timestamp)?; @@ -381,7 +381,7 @@ impl<'a> ActorBridge for VehicleBridge<'a> { } } -impl<'a> Drop for VehicleBridge<'a> { +impl Drop for VehicleBridge<'_> { fn drop(&mut self) { log::info!("Remove vehicle name {}", self.vehicle_name()); } diff --git a/src/clock.rs b/src/clock.rs index adedfb1..388a402 100644 --- a/src/clock.rs +++ b/src/clock.rs @@ -1,7 +1,9 @@ use crate::error::Result; use cdr::{CdrLe, Infinite}; -use std::sync::Arc; -use std::time::{SystemTime, UNIX_EPOCH}; +use std::{ + sync::Arc, + time::{SystemTime, UNIX_EPOCH}, +}; use zenoh::{pubsub::Publisher, Session, Wait}; use zenoh_ros_type::{builtin_interfaces, rosgraph_msgs}; @@ -20,7 +22,7 @@ impl<'a> SimulatorClock<'a> { let time = if let Some(sec) = timestamp { builtin_interfaces::Time { sec: sec.floor() as i32, - nanosec: (sec.fract() * 1000_000_000_f64) as u32, + nanosec: (sec.fract() * 1_000_000_000_f64) as u32, } } else { // If there is no timestamp, use system time diff --git a/src/main.rs b/src/main.rs index feb52c5..ed9fe24 100644 --- a/src/main.rs +++ b/src/main.rs @@ -25,6 +25,7 @@ use zenoh::{Config, Wait}; // The default interval between ticks const DEFAULT_CARLA_TICK_INTERVAL_MS: &str = "50"; +#[allow(clippy::upper_case_acronyms)] #[derive(Debug, Clone, PartialEq, ValueEnum)] enum Mode { /// Using zenoh-bridge-dds @@ -141,18 +142,18 @@ fn main() -> Result<()> { for id in added_ids { let actor = actor_list.remove(&id).expect("ID should be in the list!"); let bridge = match bridge::actor_bridge::get_bridge_type(actor.clone()) { - Ok(BridgeType::BridgeTypeVehicle(vehicle_name)) => { + Ok(BridgeType::Vehicle(vehicle_name)) => { let aw = autoware_list.entry(vehicle_name.clone()).or_insert( autoware::Autoware::new(vehicle_name.clone(), mode == Mode::ROS2), ); bridge::actor_bridge::create_bridge( z_session.clone(), actor, - BridgeType::BridgeTypeVehicle(vehicle_name), + BridgeType::Vehicle(vehicle_name), aw, )? } - Ok(BridgeType::BridgeTypeSensor(vehicle_name, sensor_type, sensor_name)) => { + Ok(BridgeType::Sensor(vehicle_name, sensor_type, sensor_name)) => { let aw = autoware_list.entry(vehicle_name.clone()).or_insert( autoware::Autoware::new(vehicle_name.clone(), mode == Mode::ROS2), ); @@ -160,7 +161,7 @@ fn main() -> Result<()> { bridge::actor_bridge::create_bridge( z_session.clone(), actor, - BridgeType::BridgeTypeSensor(vehicle_name, sensor_type, sensor_name), + BridgeType::Sensor(vehicle_name, sensor_type, sensor_name), aw, )? } diff --git a/src/types.rs b/src/types.rs index 6d33525..b16cb47 100644 --- a/src/types.rs +++ b/src/types.rs @@ -14,17 +14,17 @@ pub enum PointFieldType { #[derive(Debug)] #[allow(dead_code)] pub enum GnssStatus { - StatusNoFix = -1, - StatusFix = 0, - StatusSbasFix = 1, - StatusGbasFix = 2, + NoFix = -1, + Fix = 0, + SbasFix = 1, + GbasFix = 2, } #[derive(Debug)] #[allow(dead_code)] pub enum GnssService { - ServiceGps = 1, - ServiceGlonass = 2, - ServiceCompass = 4, - ServiceGalileo = 8, + Gps = 1, + Glonass = 2, + Compass = 4, + Galileo = 8, } diff --git a/src/utils.rs b/src/utils.rs index 40978f0..03030e6 100644 --- a/src/utils.rs +++ b/src/utils.rs @@ -9,7 +9,7 @@ pub fn create_ros_header(timestamp: Option) -> std_msgs::Header { let time = if let Some(sec) = timestamp { builtin_interfaces::Time { sec: sec.floor() as i32, - nanosec: (sec.fract() * 1000_000_000_f64) as u32, + nanosec: (sec.fract() * 1_000_000_000_f64) as u32, } } else { // If there is no timestamp, use system time