From 07514d7e92c0101d3be9cf5c7dbfab61e1ec273e Mon Sep 17 00:00:00 2001 From: Martin Dahl Date: Mon, 15 May 2023 17:22:13 +0200 Subject: [PATCH] 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 19925ec..f37834c 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 ecd9a19..6e62785 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 84c5981..af90110 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 3e11f26..3d0f397 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 012013c..f057e79 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); + } }