Merge pull request ros2-rust#3 from fmrico/fix_change_state

jhdcs · web-flow · commit eceda8094080 · 2024-01-09T12:59:34.000-05:00
Fix change state
diff --git a/rclrs/src/lifecycle_node.rs b/rclrs/src/lifecycle_node.rs
@@ -142,7 +142,44 @@ impl LifecycleNode {
     /// See [`LifecycleNodeBuilder::new()`] for documentation.
     #[allow(clippy::new_ret_no_self)]
     pub fn new(context: &Context, node_name: &str) -> Result<LifecycleNode, Box<dyn Error>> {
-        Self::builder(context, node_name).build()
+        
+        let mut node = Self::builder(context, node_name);
+
+        // `Activating` transition callback
+        let on_activate_cb = |_: &State| Transition::TRANSITION_CALLBACK_SUCCESS;
+        let on_activate: LifecycleCallback = Box::new(on_activate_cb);
+
+        // `Cleanup` transition callback
+        let on_cleanup_cb = |_: &State| Transition::TRANSITION_CALLBACK_SUCCESS;
+        let on_cleanup: LifecycleCallback = Box::new(on_cleanup_cb);
+
+        // `Configuring` transition callback
+        let on_configure_cb = |_: &State| Transition::TRANSITION_CALLBACK_SUCCESS;
+        let on_configure: LifecycleCallback = Box::new(on_configure_cb);
+
+        // `Deactivate` transition callback
+        let on_deactivate_cb = |_: &State| Transition::TRANSITION_CALLBACK_SUCCESS;
+        let on_deactivate: LifecycleCallback = Box::new(on_deactivate_cb);
+
+        // Error handling callback
+        let on_error_cb = |_: &State| Transition::TRANSITION_CALLBACK_SUCCESS;
+        let on_error: LifecycleCallback = Box::new(on_error_cb);
+
+        // `Shutdown` transition callback
+        let on_shutdown_cb = |_: &State| Transition::TRANSITION_CALLBACK_SUCCESS;
+        let on_shutdown: LifecycleCallback = Box::new(on_shutdown_cb);
+
+        node.on_activate = Some(on_activate);
+        node.on_cleanup = Some(on_cleanup);
+        node.on_configure = Some(on_configure);
+        node.on_deactivate = Some(on_deactivate);
+        node.on_error = Some(on_error);
+        node.on_shutdown = Some(on_shutdown);
+
+        node.enable_communication_interface = true;
+
+        // Build node
+        node.build()
     }
 
     /// Returns the name of the lifecycle node.
@@ -157,7 +194,7 @@ impl LifecycleNode {
     ///
     /// This returns the namespace after remapping, so it is not necessarily the same as the
     /// namespace that was used when creating the lifecycle node.
-    pub fn namepsace(&self) -> String {
+    pub fn namespace(&self) -> String {
         self.call_string_getter(rcl_node_get_namespace)
     }
 
@@ -393,6 +430,53 @@ mod tests {
     // const NODE_GET_AVAILABLE_TRANSITIONS_TOPIC: &str = "/lc_talker/get_available_transitions";
     // const NODE_GET_TRANSITION_GRAPH_TOPIC: &str = "/lc_talker/get_transition_graph";
 
+
+    #[test]
+    fn empty_initializer() {
+        let context = Context::new(vec![]).unwrap();
+        let mut test_node = LifecycleNode::new(&context, "testnode").unwrap();
+
+        assert_eq!(test_node.name(), "testnode");
+        assert_eq!(test_node.namespace(), "/");
+        assert_eq!(test_node.fully_qualified_name(), "/testnode");
+    }
+
+    #[test]
+    fn trigger_transition() {
+        let context = Context::new(vec![]).unwrap();
+        let mut test_node = LifecycleNode::new(&context, "testnode").unwrap();
+        
+        assert_eq!(
+             lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+             test_node.state_machine.get_current_state().unwrap().id());
+ 
+        assert_eq!(
+            lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+            test_node.state_machine.change_state(Transition::TRANSITION_CONFIGURE)
+                .unwrap().id());
+
+       assert_eq!(
+            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+            test_node.state_machine.change_state(Transition::TRANSITION_ACTIVATE)
+               .unwrap().id());
+
+        assert_eq!(
+            lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+            test_node.state_machine.change_state(Transition::TRANSITION_DEACTIVATE)
+                .unwrap().id());
+
+        assert_eq!(
+            lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+            test_node.state_machine.change_state(Transition::TRANSITION_CLEANUP)
+                .unwrap().id());               
+
+        assert_eq!(
+            lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
+            test_node.state_machine.change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN)
+                .unwrap().id());   
+    }
+
+    
     #[test]
     fn lifecycle_node_is_send_and_sync() {
         assert_send::<LifecycleNode>();
@@ -419,7 +503,7 @@ mod tests {
     // TODO(jhassold): Create tests to see if lifecycle node responds appropriately to messages for each type of state.
 
     #[test]
-    fn test_trigger_transition() {
+    fn trigger_transition_built() {
         let context = Context::new(vec![]).unwrap();
         let mut test_node_builder = LifecycleNode::builder(&context, "test_node");
 
@@ -457,12 +541,17 @@ mod tests {
         test_node_builder.enable_communication_interface = true;
 
         let test_node = test_node_builder.build().unwrap();
-        println!("{:?}", test_node);
-        assert_eq!(Transition::TRANSITION_CALLBACK_SUCCESS, test_node.state_machine.change_state(Transition::TRANSITION_CONFIGURE).unwrap());
+        assert_eq!(
+            lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+            test_node.state_machine.change_state(Transition::TRANSITION_CONFIGURE).unwrap().id());
         let q = test_node.state_machine.get_current_state().unwrap();
-        println!("{:?}", q);
-        // assert_eq!(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_node.state_machine.change_state(Transition::TRANSITION_ACTIVATE).unwrap());
-        // assert_eq!(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_node.state_machine.change_state(Transition::TRANSITION_DEACTIVATE).unwrap());
-        // assert_eq!(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_node.state_machine.change_state(Transition::TRANSITION_CLEANUP).unwrap());
+        assert_eq!(
+            lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
+            test_node.state_machine.change_state(Transition::TRANSITION_ACTIVATE).unwrap().id());
+        assert_eq!(
+            lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+            test_node.state_machine.change_state(Transition::TRANSITION_DEACTIVATE).unwrap().id());
+        assert_eq!(lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
+            test_node.state_machine.change_state(Transition::TRANSITION_CLEANUP).unwrap().id());
     }
 }
diff --git a/rclrs/src/lifecycle_node/state.rs b/rclrs/src/lifecycle_node/state.rs
@@ -27,6 +27,7 @@ pub struct State {
     label: String,
     allocator: rcutils_allocator_t,
     state_handle: Arc<Mutex<*mut rcl_lifecycle_state_t>>,
+    from_raw: bool,
 }
 
 impl Drop for State {
@@ -73,6 +74,7 @@ impl State {
                 label: state_label.to_string_lossy().to_string(),
                 allocator,
                 state_handle: Arc::new(Mutex::new(state_handle)),
+                from_raw: false,
             })
         }
     }
@@ -90,6 +92,7 @@ impl State {
             label: label.to_string_lossy().to_string(),
             allocator,
             state_handle: Arc::new(Mutex::new(rcl_lifecycle_state_handle)),
+            from_raw: true,
         }
     }
 
@@ -108,9 +111,11 @@ impl State {
             return Ok(());
         }
 
-        // SAFETY: By this point, we should have confirmed that state_handle still exists
-        unsafe {
-            rcl_lifecycle_state_fini(state_handle.as_mut().unwrap(), &self.allocator).ok()?;
+        if !self.from_raw {
+            // SAFETY: By this point, we should have confirmed that state_handle still exists
+            unsafe {
+                rcl_lifecycle_state_fini(state_handle.as_mut().unwrap(), &self.allocator).ok()?;
+            }
         }
 
         Ok(())
diff --git a/rclrs/src/lifecycle_node/state_machine.rs b/rclrs/src/lifecycle_node/state_machine.rs
@@ -89,7 +89,7 @@ impl LifecycleMachine {
         };
 
         let ret = self.change_state(transition_id).unwrap();
-        resp.success = ret == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;
+        resp.success = ret.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;
 
         resp
     }
@@ -403,7 +403,7 @@ impl LifecycleMachine {
         }
     }
     
-    pub(crate) fn change_state(&self, transition_id: u8) -> Result<u8, RclrsError> {
+    pub(crate) fn change_state(&self, transition_id: u8) -> Result<State, RclrsError> {
         // Make sure that the state machine is initialized before doing anything
         let mut state_machine = self.state_machine.lock().unwrap();
         // SAFETY: No preconditions for this function
@@ -446,7 +446,10 @@ impl LifecycleMachine {
             .ok()?
         };
 
-        Ok(cb_return_code)
+        let current_state =
+            unsafe { State::from_raw(state_machine.current_state as *mut rcl_lifecycle_state_s) };
+        
+        Ok(current_state)
     }
 
     fn execute_callback(&self, cb_id: u8, previous_state: &State) -> u8 {
diff --git a/rclrs/src/lifecycle_node/transition.rs b/rclrs/src/lifecycle_node/transition.rs
@@ -31,6 +31,7 @@ pub struct Transition {
     goal: Option<State>,
     allocator: rcutils_allocator_t,
     transition_handle: Arc<Mutex<*mut rcl_lifecycle_transition_t>>,
+    from_raw: bool,
 }
 
 impl Drop for Transition {
@@ -87,6 +88,7 @@ impl Transition {
                 goal: None,
                 allocator,
                 transition_handle: Arc::new(Mutex::new(transition_handle)),
+                from_raw: false,
             })
         }
     }
@@ -134,6 +136,7 @@ impl Transition {
             goal,
             allocator,
             transition_handle: Arc::new(Mutex::new(rcl_lifecycle_transition_handle)),
+            from_raw: true,
         }
     }
 
@@ -160,10 +163,12 @@ impl Transition {
             return Ok(());
         }
 
-        // SAFETY: By this point, we should have confirmed that transition_handle still exists
-        unsafe {
-            rcl_lifecycle_transition_fini(transition_handle.as_mut().unwrap(), &self.allocator)
-                .ok()?;
+        if !self.from_raw {
+            // SAFETY: By this point, we should have confirmed that transition_handle still exists
+            unsafe {
+                rcl_lifecycle_transition_fini(transition_handle.as_mut().unwrap(), &self.allocator)
+                    .ok()?;
+            }
         }
 
         Ok(())

Original file line number	Diff line number	Diff line change
`@@ -27,6 +27,7 @@ pub struct State {`
`27`	`27`	`label: String,`
`28`	`28`	`allocator: rcutils_allocator_t,`
`29`	`29`	`state_handle: Arc<Mutex<*mut rcl_lifecycle_state_t>>,`
	`30`	`+ from_raw: bool,`
`30`	`31`	`}`
`31`	`32`
`32`	`33`	`impl Drop for State {`
`@@ -73,6 +74,7 @@ impl State {`
`73`	`74`	`label: state_label.to_string_lossy().to_string(),`
`74`	`75`	`allocator,`
`75`	`76`	`state_handle: Arc::new(Mutex::new(state_handle)),`
	`77`	`+ from_raw: false,`
`76`	`78`	`})`
`77`	`79`	`}`
`78`	`80`	`}`
`@@ -90,6 +92,7 @@ impl State {`
`90`	`92`	`label: label.to_string_lossy().to_string(),`
`91`	`93`	`allocator,`
`92`	`94`	`state_handle: Arc::new(Mutex::new(rcl_lifecycle_state_handle)),`
	`95`	`+ from_raw: true,`
`93`	`96`	`}`
`94`	`97`	`}`
`95`	`98`
`@@ -108,9 +111,11 @@ impl State {`
`108`	`111`	`return Ok(());`
`109`	`112`	`}`
`110`	`113`
`111`		`- // SAFETY: By this point, we should have confirmed that state_handle still exists`
`112`		`- unsafe {`
`113`		`- rcl_lifecycle_state_fini(state_handle.as_mut().unwrap(), &self.allocator).ok()?;`
	`114`	`+ if !self.from_raw {`
	`115`	`+ // SAFETY: By this point, we should have confirmed that state_handle still exists`
	`116`	`+ unsafe {`
	`117`	`+ rcl_lifecycle_state_fini(state_handle.as_mut().unwrap(), &self.allocator).ok()?;`
	`118`	`+ }`
`114`	`119`	`}`
`115`	`120`
`116`	`121`	`Ok(())`
Original file line number	Diff line number	Diff line change
`@@ -89,7 +89,7 @@ impl LifecycleMachine {`
`89`	`89`	`};`
`90`	`90`
`91`	`91`	`let ret = self.change_state(transition_id).unwrap();`
`92`		`- resp.success = ret == lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS;`
	`92`	`+ resp.success = ret.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED;`
`93`	`93`
`94`	`94`	`resp`
`95`	`95`	`}`
`@@ -403,7 +403,7 @@ impl LifecycleMachine {`
`403`	`403`	`}`
`404`	`404`	`}`
`405`	`405`
`406`		`- pub(crate) fn change_state(&self, transition_id: u8) -> Result<u8, RclrsError> {`
	`406`	`+ pub(crate) fn change_state(&self, transition_id: u8) -> Result<State, RclrsError> {`
`407`	`407`	`// Make sure that the state machine is initialized before doing anything`
`408`	`408`	`let mut state_machine = self.state_machine.lock().unwrap();`
`409`	`409`	`// SAFETY: No preconditions for this function`
`@@ -446,7 +446,10 @@ impl LifecycleMachine {`
`446`	`446`	`.ok()?`
`447`	`447`	`};`
`448`	`448`
`449`		`- Ok(cb_return_code)`
	`449`	`+ let current_state =`
	`450`	`+ unsafe { State::from_raw(state_machine.current_state as *mut rcl_lifecycle_state_s) };`
	`451`	`+`
	`452`	`+ Ok(current_state)`
`450`	`453`	`}`
`451`	`454`
`452`	`455`	`fn execute_callback(&self, cb_id: u8, previous_state: &State) -> u8 {`
Original file line number	Diff line number	Diff line change
`@@ -31,6 +31,7 @@ pub struct Transition {`
`31`	`31`	`goal: Option<State>,`
`32`	`32`	`allocator: rcutils_allocator_t,`
`33`	`33`	`transition_handle: Arc<Mutex<*mut rcl_lifecycle_transition_t>>,`
	`34`	`+ from_raw: bool,`
`34`	`35`	`}`
`35`	`36`
`36`	`37`	`impl Drop for Transition {`
`@@ -87,6 +88,7 @@ impl Transition {`
`87`	`88`	`goal: None,`
`88`	`89`	`allocator,`
`89`	`90`	`transition_handle: Arc::new(Mutex::new(transition_handle)),`
	`91`	`+ from_raw: false,`
`90`	`92`	`})`
`91`	`93`	`}`
`92`	`94`	`}`
`@@ -134,6 +136,7 @@ impl Transition {`
`134`	`136`	`goal,`
`135`	`137`	`allocator,`
`136`	`138`	`transition_handle: Arc::new(Mutex::new(rcl_lifecycle_transition_handle)),`
	`139`	`+ from_raw: true,`
`137`	`140`	`}`
`138`	`141`	`}`
`139`	`142`
`@@ -160,10 +163,12 @@ impl Transition {`
`160`	`163`	`return Ok(());`
`161`	`164`	`}`
`162`	`165`
`163`		`- // SAFETY: By this point, we should have confirmed that transition_handle still exists`
`164`		`- unsafe {`
`165`		`- rcl_lifecycle_transition_fini(transition_handle.as_mut().unwrap(), &self.allocator)`
`166`		`- .ok()?;`
	`166`	`+ if !self.from_raw {`
	`167`	`+ // SAFETY: By this point, we should have confirmed that transition_handle still exists`
	`168`	`+ unsafe {`
	`169`	`+ rcl_lifecycle_transition_fini(transition_handle.as_mut().unwrap(), &self.allocator)`
	`170`	`+ .ok()?;`
	`171`	`+ }`
`167`	`172`	`}`
`168`	`173`
`169`	`174`	`Ok(())`