From 16f73aed4b9f7f4a47e981f6a6bccf34568a9bf2 Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Mon, 15 May 2023 15:40:14 +0200 Subject: [PATCH 1/3] Adds a hack to generate constants associated to the message types. --- r2r/src/msg_types.rs | 10 ++++++++ r2r_msg_gen/build.rs | 53 ++++++++++++++++++++++++++++++++++++++++++ r2r_msg_gen/src/lib.rs | 24 +++++++++++++++++-- 3 files changed, 85 insertions(+), 2 deletions(-) diff --git a/r2r/src/msg_types.rs b/r2r/src/msg_types.rs index 96d2141f3..dbd2557b0 100644 --- a/r2r/src/msg_types.rs +++ b/r2r/src/msg_types.rs @@ -770,4 +770,14 @@ mod tests { // the message should contain something (default msg) assert!(!json_request.to_string().is_empty()); } + + + #[cfg(r2r__action_msgs__msg__GoalStatus)] + #[test] + fn test_msg_constants() { + use action_msgs::msg::GoalStatus; + let gs = GoalStatus::default(); + + assert_eq!(gs.status, GoalStatus::STATUS_UNKNOWN as i8); + } } diff --git a/r2r_msg_gen/build.rs b/r2r_msg_gen/build.rs index 7d8ab582b..f28445d92 100644 --- a/r2r_msg_gen/build.rs +++ b/r2r_msg_gen/build.rs @@ -4,13 +4,16 @@ use r2r_common::RosMsg; use std::fs::OpenOptions; use std::path::{Path, PathBuf}; use std::{env, fs}; +use std::collections::HashMap; const MSG_INCLUDES_FILENAME: &str = "msg_includes.h"; const INTROSPECTION_FILENAME: &str = "introspection_functions.rs"; +const CONSTANTS_FILENAME: &str = "constants.rs"; const BINDINGS_FILENAME: &str = "msg_bindings.rs"; const GENERATED_FILES: &[&str] = &[ MSG_INCLUDES_FILENAME, INTROSPECTION_FILENAME, + CONSTANTS_FILENAME, BINDINGS_FILENAME, ]; @@ -76,6 +79,7 @@ fn run_bindgen(msg_list: &[RosMsg]) { fn generate_bindings(bindgen_dir: &Path, msg_list: &[RosMsg]) { let msg_includes_file = bindgen_dir.join(MSG_INCLUDES_FILENAME); let introspection_file = bindgen_dir.join(INTROSPECTION_FILENAME); + let constants_file = bindgen_dir.join(CONSTANTS_FILENAME); let bindings_file = bindgen_dir.join(BINDINGS_FILENAME); let mut includes = String::new(); @@ -170,6 +174,55 @@ fn generate_bindings(bindgen_dir: &Path, msg_list: &[RosMsg]) { let bindings = builder.generate().expect("Unable to generate bindings"); + // Let's add a hack to generate constants. + let str_bindings = bindings.to_string(); + // find all lines which look suspiciosly like a constant. + let mut constants: HashMap> = HashMap::new(); + for msg in msg_list { + let prefix = &format!("pub const {}__{}__{}__", &msg.module, &msg.prefix, &msg.name); + let mut lines = str_bindings.lines(); + while let Some(line) = lines.next() { + if let Some(constant) = line.strip_prefix(prefix) { + if let Some((con, typ)) = constant.split_once(":") { + // These are generated automatically for arrays and strings, we don't need to expose them. + if con.ends_with("__MAX_SIZE") || con.ends_with("__MAX_STRING_SIZE") { + continue; + } + let key = format!("{}__{}__{}", msg.module, msg.prefix, msg.name); + if let Some((t, _)) = typ.split_once("=") { + constants.entry(key).or_default().push((con.to_string(), t.trim().to_string())); + } else if let Some(next_line) = lines.next() { + // type has moved down to the next line. (bindgen has a max line width) + if let Some((t, _)) = next_line.split_once("=") { + constants.entry(key).or_default().push((con.to_string(), t.trim().to_string())); + } else { + panic!("Code generation failure. Type not found in line! {}", next_line); + } + } else { + panic!("Code generation failure. Type not found in line! {}", line); + } + } + } + } + } + // generate a constant which holds all constants. + let mut constants_map = String::from( + "\ + lazy_static! { + static ref CONSTANTS_MAP: HashMap<&'static str, Vec<(String,String)>> = { + let mut m = HashMap::new();\ +"); + for (msg,msg_constants) in constants { + let msg_constants_str = format!("vec![{}]", + msg_constants.iter() + .map(|(c,t)| format!("(\"{c}\".to_string(), \"{t}\".to_string())")) + .collect::>().join(",")); + + constants_map.push_str(&format!("m.insert(\"{}\", {});\n", msg, msg_constants_str)); + } + constants_map.push_str("m \n }; }\n\n"); + fs::write(&constants_file, constants_map).unwrap(); + bindings .write_to_file(bindings_file) .expect("Couldn't write bindings!"); diff --git a/r2r_msg_gen/src/lib.rs b/r2r_msg_gen/src/lib.rs index 8c730eeb9..a4c9bd84d 100644 --- a/r2r_msg_gen/src/lib.rs +++ b/r2r_msg_gen/src/lib.rs @@ -6,6 +6,7 @@ #![allow(clippy::all)] include!(concat!(env!("OUT_DIR"), "/msg_bindings.rs")); include!(concat!(env!("OUT_DIR"), "/introspection_functions.rs")); +include!(concat!(env!("OUT_DIR"), "/constants.rs")); #[macro_use] extern crate lazy_static; @@ -602,6 +603,23 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { msgname = name ); + + let constants = CONSTANTS_MAP.get(key.as_str()).cloned().unwrap_or_default(); + let mut constant_strings = vec![]; + for (c, typ) in constants { + constant_strings.push(format!(" pub const {c}: {typ} = {key}__{c};")); + } + let impl_constants = format!(" + #[allow(non_upper_case_globals)] + impl {msgname} {{ + {constants} + }} + ", + msgname = name, + constants = constant_strings.join("\n") + ); + + let module_str = format!( " #[derive(Clone,Debug,PartialEq,Serialize,Deserialize)] @@ -610,12 +628,14 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { {fields} }}\n {typesupport}\n - {default}\n\n + {default}\n + {constants}\n\n ", msgname = name, fields = fields, typesupport = typesupport, - default = impl_default + default = impl_default, + constants = impl_constants, ); module_str From 0ae3c80dcb45d73868ed02f027853d7173e8f360 Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Mon, 15 May 2023 16:50:33 +0200 Subject: [PATCH 2/3] Extend the hack to generate constants for service and action types. --- r2r/src/msg_types.rs | 5 ++++ r2r_msg_gen/build.rs | 62 ++++++++++++++++++++++++++---------------- r2r_msg_gen/src/lib.rs | 9 ++++-- 3 files changed, 50 insertions(+), 26 deletions(-) diff --git a/r2r/src/msg_types.rs b/r2r/src/msg_types.rs index dbd2557b0..012013c72 100644 --- a/r2r/src/msg_types.rs +++ b/r2r/src/msg_types.rs @@ -779,5 +779,10 @@ mod tests { let gs = GoalStatus::default(); assert_eq!(gs.status, GoalStatus::STATUS_UNKNOWN as i8); + + use action_msgs::srv::CancelGoal; + let cgr = CancelGoal::Response::default(); + + assert_eq!(cgr.return_code, CancelGoal::Response::ERROR_NONE as i8); } } diff --git a/r2r_msg_gen/build.rs b/r2r_msg_gen/build.rs index f28445d92..993bc1be4 100644 --- a/r2r_msg_gen/build.rs +++ b/r2r_msg_gen/build.rs @@ -179,30 +179,19 @@ fn generate_bindings(bindgen_dir: &Path, msg_list: &[RosMsg]) { // find all lines which look suspiciosly like a constant. let mut constants: HashMap> = HashMap::new(); for msg in msg_list { - let prefix = &format!("pub const {}__{}__{}__", &msg.module, &msg.prefix, &msg.name); - let mut lines = str_bindings.lines(); - while let Some(line) = lines.next() { - if let Some(constant) = line.strip_prefix(prefix) { - if let Some((con, typ)) = constant.split_once(":") { - // These are generated automatically for arrays and strings, we don't need to expose them. - if con.ends_with("__MAX_SIZE") || con.ends_with("__MAX_STRING_SIZE") { - continue; - } - let key = format!("{}__{}__{}", msg.module, msg.prefix, msg.name); - if let Some((t, _)) = typ.split_once("=") { - constants.entry(key).or_default().push((con.to_string(), t.trim().to_string())); - } else if let Some(next_line) = lines.next() { - // type has moved down to the next line. (bindgen has a max line width) - if let Some((t, _)) = next_line.split_once("=") { - constants.entry(key).or_default().push((con.to_string(), t.trim().to_string())); - } else { - panic!("Code generation failure. Type not found in line! {}", next_line); - } - } else { - panic!("Code generation failure. Type not found in line! {}", line); - } - } + if msg.prefix == "srv" { + for s in &["Request", "Response"] { + let key = format!("{}__{}__{}_{}", &msg.module, &msg.prefix, &msg.name, s); + add_constants(&key, &str_bindings, &mut constants); } + } else if msg.prefix == "action" { + for s in &["Goal", "Result", "Feedback", "FeedbackMessage"] { + let key = format!("{}__{}__{}_{}", &msg.module, &msg.prefix, &msg.name, s); + add_constants(&key, &str_bindings, &mut constants); + } + } else { + let key = format!("{}__{}__{}", &msg.module, &msg.prefix, &msg.name); + add_constants(&key, &str_bindings, &mut constants); } } // generate a constant which holds all constants. @@ -228,6 +217,33 @@ fn generate_bindings(bindgen_dir: &Path, msg_list: &[RosMsg]) { .expect("Couldn't write bindings!"); } +fn add_constants(key: &str, bindings: &str, constants: &mut HashMap>) { + let mut lines = bindings.lines(); + while let Some(line) = lines.next() { + let prefix = format!("pub const {}__", key); + if let Some(constant) = line.strip_prefix(&prefix) { + if let Some((con, typ)) = constant.split_once(":") { + // These are generated automatically for arrays and strings, we don't need to expose them. + if con.ends_with("__MAX_SIZE") || con.ends_with("__MAX_STRING_SIZE") { + continue; + } + if let Some((t, _)) = typ.split_once("=") { + constants.entry(key.into()).or_default().push((con.to_string(), t.trim().to_string())); + } else if let Some(next_line) = lines.next() { + // type has moved down to the next line. (bindgen has a max line width) + if let Some((t, _)) = next_line.split_once("=") { + constants.entry(key.into()).or_default().push((con.to_string(), t.trim().to_string())); + } else { + panic!("Code generation failure. Type not found in line! {}", next_line); + } + } else { + panic!("Code generation failure. Type not found in line! {}", line); + } + } + } + } +} + fn run_dynlink(#[allow(unused_variables)] msg_list: &[RosMsg]) { #[cfg(not(feature = "doc-only"))] { diff --git a/r2r_msg_gen/src/lib.rs b/r2r_msg_gen/src/lib.rs index a4c9bd84d..fa3d9dd83 100644 --- a/r2r_msg_gen/src/lib.rs +++ b/r2r_msg_gen/src/lib.rs @@ -609,15 +609,18 @@ pub fn generate_rust_msg(module_: &str, prefix_: &str, name_: &str) -> String { for (c, typ) in constants { constant_strings.push(format!(" pub const {c}: {typ} = {key}__{c};")); } - let impl_constants = format!(" + let impl_constants = if constant_strings.is_empty() { + String::new() + } else { + format!(" #[allow(non_upper_case_globals)] impl {msgname} {{ {constants} }} ", msgname = name, - constants = constant_strings.join("\n") - ); + constants = constant_strings.join("\n")) + }; let module_str = format!( From 07514d7e92c0101d3be9cf5c7dbfab61e1ec273e Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Mon, 15 May 2023 17:22:13 +0200 Subject: [PATCH 3/3] Use generated constants to instead of magic numbers for ros actions --- r2r/src/action_clients.rs | 8 ++++---- r2r/src/action_clients_untyped.rs | 8 ++++---- r2r/src/action_common.rs | 28 ++++++++++++++-------------- r2r/src/action_servers.rs | 17 +++++++---------- r2r/src/msg_types.rs | 12 ++++++++++++ 5 files changed, 41 insertions(+), 32 deletions(-) diff --git a/r2r/src/action_clients.rs b/r2r/src/action_clients.rs index 19925ecb8..f37834c42 100644 --- a/r2r/src/action_clients.rs +++ b/r2r/src/action_clients.rs @@ -250,10 +250,10 @@ where .map_err(|_| Error::RCL_RET_CLIENT_INVALID) .map(|r| match r { Ok(r) => match r.return_code { - 0 => Ok(()), - 1 => Err(Error::GoalCancelRejected), - 2 => Err(Error::GoalCancelUnknownGoalID), - 3 => Err(Error::GoalCancelAlreadyTerminated), + e if e == action_msgs::srv::CancelGoal::Response::ERROR_NONE as i8 => Ok(()), + e if e == action_msgs::srv::CancelGoal::Response::ERROR_REJECTED as i8 => Err(Error::GoalCancelRejected), + e if e == action_msgs::srv::CancelGoal::Response::ERROR_UNKNOWN_GOAL_ID as i8 => Err(Error::GoalCancelUnknownGoalID), + e if e == action_msgs::srv::CancelGoal::Response::ERROR_GOAL_TERMINATED as i8 => Err(Error::GoalCancelAlreadyTerminated), x => panic!("unknown error code return from action server: {}", x), }, Err(e) => Err(e), diff --git a/r2r/src/action_clients_untyped.rs b/r2r/src/action_clients_untyped.rs index ecd9a19d7..6e6278514 100644 --- a/r2r/src/action_clients_untyped.rs +++ b/r2r/src/action_clients_untyped.rs @@ -217,10 +217,10 @@ impl WrappedActionClientUntyped { .map_err(|_| Error::RCL_RET_CLIENT_INVALID) .map(|r| match r { Ok(r) => match r.return_code { - 0 => Ok(()), - 1 => Err(Error::GoalCancelRejected), - 2 => Err(Error::GoalCancelUnknownGoalID), - 3 => Err(Error::GoalCancelAlreadyTerminated), + e if e == action_msgs::srv::CancelGoal::Response::ERROR_NONE as i8 => Ok(()), + e if e == action_msgs::srv::CancelGoal::Response::ERROR_REJECTED as i8 => Err(Error::GoalCancelRejected), + e if e == action_msgs::srv::CancelGoal::Response::ERROR_UNKNOWN_GOAL_ID as i8 => Err(Error::GoalCancelUnknownGoalID), + e if e == action_msgs::srv::CancelGoal::Response::ERROR_GOAL_TERMINATED as i8 => Err(Error::GoalCancelAlreadyTerminated), x => panic!("unknown error code return from action server: {}", x), }, Err(e) => Err(e), diff --git a/r2r/src/action_common.rs b/r2r/src/action_common.rs index 84c598152..af9011068 100644 --- a/r2r/src/action_common.rs +++ b/r2r/src/action_common.rs @@ -14,25 +14,25 @@ impl GoalStatus { #[allow(dead_code)] pub fn to_rcl(&self) -> i8 { match self { - GoalStatus::Unknown => 0, - GoalStatus::Accepted => 1, - GoalStatus::Executing => 2, - GoalStatus::Canceling => 3, - GoalStatus::Succeeded => 4, - GoalStatus::Canceled => 5, - GoalStatus::Aborted => 6, + GoalStatus::Unknown => crate::action_msgs::msg::GoalStatus::STATUS_UNKNOWN as i8, + GoalStatus::Accepted => crate::action_msgs::msg::GoalStatus::STATUS_ACCEPTED as i8, + GoalStatus::Executing => crate::action_msgs::msg::GoalStatus::STATUS_EXECUTING as i8, + GoalStatus::Canceling => crate::action_msgs::msg::GoalStatus::STATUS_CANCELING as i8, + GoalStatus::Succeeded => crate::action_msgs::msg::GoalStatus::STATUS_SUCCEEDED as i8, + GoalStatus::Canceled => crate::action_msgs::msg::GoalStatus::STATUS_CANCELED as i8, + GoalStatus::Aborted => crate::action_msgs::msg::GoalStatus::STATUS_ABORTED as i8, } } pub fn from_rcl(s: i8) -> Self { match s { - 0 => GoalStatus::Unknown, - 1 => GoalStatus::Accepted, - 2 => GoalStatus::Executing, - 3 => GoalStatus::Canceling, - 4 => GoalStatus::Succeeded, - 5 => GoalStatus::Canceled, - 6 => GoalStatus::Aborted, + s if s == crate::action_msgs::msg::GoalStatus::STATUS_UNKNOWN as i8 => GoalStatus::Unknown, + s if s == crate::action_msgs::msg::GoalStatus::STATUS_ACCEPTED as i8 => GoalStatus::Accepted, + s if s == crate::action_msgs::msg::GoalStatus::STATUS_EXECUTING as i8 => GoalStatus::Executing, + s if s == crate::action_msgs::msg::GoalStatus::STATUS_CANCELING as i8 => GoalStatus::Canceling, + s if s == crate::action_msgs::msg::GoalStatus::STATUS_SUCCEEDED as i8 => GoalStatus::Succeeded, + s if s == crate::action_msgs::msg::GoalStatus::STATUS_CANCELED as i8 => GoalStatus::Canceled, + s if s == crate::action_msgs::msg::GoalStatus::STATUS_ABORTED as i8 => GoalStatus::Aborted, _ => panic!("unknown action status: {}", s), } } diff --git a/r2r/src/action_servers.rs b/r2r/src/action_servers.rs index 3e11f2613..3d0f39776 100644 --- a/r2r/src/action_servers.rs +++ b/r2r/src/action_servers.rs @@ -7,7 +7,6 @@ use std::ffi::CString; use std::mem::MaybeUninit; use std::sync::{Arc, Mutex, Weak}; -use crate::action_common::*; use crate::error::*; use crate::msg_types::generated_msgs::{action_msgs, builtin_interfaces, unique_identifier_msgs}; use crate::msg_types::*; @@ -192,14 +191,14 @@ where fn is_cancelling(&self, uuid: &uuid::Uuid) -> Result { if let Some(handle) = self.goals.get(uuid) { - let mut state = 0u8; // TODO: int8 STATUS_UNKNOWN = 0; + let mut state = action_msgs::msg::GoalStatus::STATUS_UNKNOWN as u8; let ret = unsafe { rcl_action_goal_handle_get_status(*handle, &mut state) }; if ret != RCL_RET_OK as i32 { println!("action server: Failed to get goal handle state: {}", ret); return Err(Error::from_rcl_error(ret)); } - return Ok(state == 3u8); // TODO: int8 STATUS_CANCELING + return Ok(state == action_msgs::msg::GoalStatus::STATUS_CANCELING as u8); } Err(Error::RCL_RET_ACTION_GOAL_HANDLE_INVALID) } @@ -295,7 +294,7 @@ where // check if all cancels were rejected. if requested_cancels >= 1 && response_msg.goals_canceling.is_empty() { - response_msg.return_code = 1; // TODO: auto generate these (int8 ERROR_REJECTED=1) + response_msg.return_code = action_msgs::srv::CancelGoal::Response::ERROR_REJECTED as i8; } responses.push((*request_id, response_msg)); @@ -534,9 +533,7 @@ where let response_msg = if !goal_exists { // Goal does not exists - println!("goal does not exist :("); - let status = GoalStatus::Unknown; - let msg = T::make_result_response_msg(status.to_rcl(), T::Result::default()); + let msg = T::make_result_response_msg(action_msgs::msg::GoalStatus::STATUS_UNKNOWN as i8, T::Result::default()); let mut response_msg = WrappedNativeMsg::< <::GetResult as WrappedServiceTypeSupport>::Response, >::from(&msg); @@ -667,7 +664,7 @@ where action_server.publish_status(); // create result message - let result_msg = T::make_result_response_msg(5, msg); // todo: int8 STATUS_CANCELED = 5 + let result_msg = T::make_result_response_msg(action_msgs::msg::GoalStatus::STATUS_CANCELED as i8, msg); let native_msg = WrappedNativeMsg::< <::GetResult as WrappedServiceTypeSupport>::Response, >::from(&result_msg); @@ -687,7 +684,7 @@ where action_server.set_goal_state(&self.uuid, rcl_action_goal_event_t::GOAL_EVENT_ABORT)?; // create result message - let result_msg = T::make_result_response_msg(6, msg); // todo: int8 STATUS_ABORTED = 6 + let result_msg = T::make_result_response_msg(action_msgs::msg::GoalStatus::STATUS_ABORTED as i8, msg); let native_msg = WrappedNativeMsg::< <::GetResult as WrappedServiceTypeSupport>::Response, >::from(&result_msg); @@ -710,7 +707,7 @@ where action_server.set_goal_state(&self.uuid, rcl_action_goal_event_t::GOAL_EVENT_SUCCEED)?; // create result message - let result_msg = T::make_result_response_msg(4, msg); // todo: int8 STATUS_SUCCEEDED = 4 + let result_msg = T::make_result_response_msg(action_msgs::msg::GoalStatus::STATUS_SUCCEEDED as i8, msg); let native_msg = WrappedNativeMsg::< <::GetResult as WrappedServiceTypeSupport>::Response, >::from(&result_msg); diff --git a/r2r/src/msg_types.rs b/r2r/src/msg_types.rs index 012013c72..f057e792e 100644 --- a/r2r/src/msg_types.rs +++ b/r2r/src/msg_types.rs @@ -779,10 +779,22 @@ mod tests { let gs = GoalStatus::default(); assert_eq!(gs.status, GoalStatus::STATUS_UNKNOWN as i8); + assert_eq!(0, GoalStatus::STATUS_UNKNOWN as i8); + assert_eq!(1, GoalStatus::STATUS_ACCEPTED as i8); + assert_eq!(2, GoalStatus::STATUS_EXECUTING as i8); + assert_eq!(3, GoalStatus::STATUS_CANCELING as i8); + assert_eq!(4, GoalStatus::STATUS_SUCCEEDED as i8); + assert_eq!(5, GoalStatus::STATUS_CANCELED as i8); + assert_eq!(6, GoalStatus::STATUS_ABORTED as i8); use action_msgs::srv::CancelGoal; let cgr = CancelGoal::Response::default(); assert_eq!(cgr.return_code, CancelGoal::Response::ERROR_NONE as i8); + assert_eq!(0, CancelGoal::Response::ERROR_NONE as i8); + assert_eq!(1, CancelGoal::Response::ERROR_REJECTED as i8); + assert_eq!(2, CancelGoal::Response::ERROR_UNKNOWN_GOAL_ID as i8); + assert_eq!(3, CancelGoal::Response::ERROR_GOAL_TERMINATED as i8); + } }