mirror
/
r2r
mirror of https://github.com/sequenceplanner/r2r.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Use generated constants to instead of magic numbers for ros actions

This commit is contained in:
Martin Dahl 2023-05-15 17:22:13 +02:00
parent 0ae3c80dcb
commit 07514d7e92
5 changed files with 41 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
r2r/src/action_clients.rs
View File

@ -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),

8
r2r/src/action_clients_untyped.rs
View File

@ -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),

28
r2r/src/action_common.rs
View File

@ -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),
}
}

17
r2r/src/action_servers.rs
View File

@ -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<bool> {
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::<
<<T as WrappedActionTypeSupport>::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::<
<<T as WrappedActionTypeSupport>::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::<
<<T as WrappedActionTypeSupport>::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::<
<<T as WrappedActionTypeSupport>::GetResult as WrappedServiceTypeSupport>::Response,
>::from(&result_msg);

12
r2r/src/msg_types.rs
View File

@ -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);
}
}