repose_motion_planning/
goal_task.rs

1//! Turn measured joint state and a configured joint goal into a planning request.
2
3use crate::CuJointPlanRequest;
4use bincode::Decode;
5use bincode::Encode;
6use bincode::de::Decoder;
7use bincode::enc::Encoder;
8use bincode::error::DecodeError;
9use bincode::error::EncodeError;
10use cu29::prelude::*;
11use repose_kinematics::CuJointPosition;
12use repose_kinematics::CuJointStatePayload;
13use repose_kinematics::CuRobotStatus;
14use serde::Deserialize;
15use serde::Serialize;
16
17/// `Option<[CuJointPosition; N]>` serde helper.
18mod option_position_array {
19    use repose_kinematics::CuJointPosition;
20    use serde::Deserialize;
21    use serde::Deserializer;
22    use serde::Serialize;
23    use serde::Serializer;
24
25    #[derive(Serialize, Deserialize)]
26    #[serde(transparent)]
27    struct Positions<const N: usize>(
28        #[serde(with = "repose_kinematics::serde_arrays::position_array")] [CuJointPosition; N],
29    );
30
31    pub(super) fn serialize<S: Serializer, const N: usize>(
32        values: &Option<[CuJointPosition; N]>,
33        serializer: S,
34    ) -> Result<S::Ok, S::Error> {
35        values.map(Positions).serialize(serializer)
36    }
37
38    pub(super) fn deserialize<'de, D: Deserializer<'de>, const N: usize>(
39        deserializer: D,
40    ) -> Result<Option<[CuJointPosition; N]>, D::Error> {
41        Ok(Option::<Positions<N>>::deserialize(deserializer)?.map(|p| p.0))
42    }
43}
44
45/// Node config of [`CuJointGoalTask`].
46///
47/// ```ron
48/// // CuJointGoalTask<2>: radians for revolute joints, meters for prismatic ones
49/// config: {"goal": [{"revolute": 0.4}, {"prismatic": 0.1}]}
50/// ```
51#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize, Reflect)]
52#[reflect(no_field_bounds, from_reflect = false)]
53// Field-level defaults: serde does not support container `default` on const-generic structs.
54#[serde(deny_unknown_fields)]
55pub struct CuJointGoalParams<const N: usize> {
56    /// Required joint goal: `N` typed joint coordinates.
57    #[serde(default, with = "option_position_array")]
58    #[reflect(ignore)]
59    pub goal: Option<[CuJointPosition; N]>,
60}
61
62impl<const N: usize> Default for CuJointGoalParams<N> {
63    fn default() -> Self {
64        Self { goal: None }
65    }
66}
67
68impl<const N: usize> CuJointGoalParams<N> {
69    /// Parameters requesting a plan to `goal`.
70    pub const fn new(goal: [CuJointPosition; N]) -> Self {
71        Self { goal: Some(goal) }
72    }
73
74    /// Checks that the goal is present.
75    pub fn validate(&self) -> CuResult<()> {
76        self.goal
77            .map(|_| ())
78            .ok_or_else(|| CuError::from("joint goal: missing required 'goal'"))
79    }
80}
81
82/// Requests one plan from the measured joint state to a configured goal.
83///
84/// Input is the robot's [`CuJointStatePayload<N>`]; output is a
85/// [`CuJointPlanRequest<N>`] for a [`crate::CuJointPlannerTask`]. The first
86/// cycle whose state reports [`CuRobotStatus::Ready`] becomes the plan start
87/// and the request is published exactly once; every other cycle publishes
88/// nothing, so the planner runs one job.
89///
90/// Node config: [`CuJointGoalParams`]. The request carries an empty planning
91/// scene; self-collision is still checked against the planner's collision
92/// model.
93#[derive(Reflect)]
94#[reflect(no_field_bounds, from_reflect = false)]
95pub struct CuJointGoalTask<const N: usize> {
96    #[reflect(ignore)]
97    request: CuJointPlanRequest<N>,
98    requested: bool,
99}
100
101impl<const N: usize> CuJointGoalTask<N> {
102    /// Creates a task that requests one plan to the configured goal.
103    pub fn from_params(params: CuJointGoalParams<N>) -> CuResult<Self> {
104        params.validate()?;
105        Ok(Self {
106            request: CuJointPlanRequest {
107                goal: params.goal.unwrap_or([CuJointPosition::default(); N]),
108                ..Default::default()
109            },
110            requested: false,
111        })
112    }
113
114    /// `true` once the request has been published.
115    pub const fn is_requested(&self) -> bool {
116        self.requested
117    }
118}
119
120impl<const N: usize> Freezable for CuJointGoalTask<N> {
121    fn freeze<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
122        Encode::encode(&self.requested, encoder)
123    }
124
125    fn thaw<D: Decoder>(&mut self, decoder: &mut D) -> Result<(), DecodeError> {
126        self.requested = Decode::decode(decoder)?;
127        Ok(())
128    }
129}
130
131impl<const N: usize> CuTask for CuJointGoalTask<N> {
132    type Resources<'r> = ();
133    type Input<'m> = input_msg!(CuJointStatePayload<N>);
134    type Output<'m> = output_msg!(CuJointPlanRequest<N>);
135
136    fn new(cfg: Option<&ComponentConfig>, _resources: Self::Resources<'_>) -> CuResult<Self> {
137        let params = cfg
138            .map(ComponentConfig::deserialize_into::<CuJointGoalParams<N>>)
139            .transpose()?
140            .unwrap_or_default();
141        Self::from_params(params)
142    }
143
144    fn process<'i, 'o>(
145        &mut self,
146        _ctx: &CuContext,
147        input: &Self::Input<'i>,
148        output: &mut Self::Output<'o>,
149    ) -> CuResult<()> {
150        output.tov = input.tov;
151        output.clear_payload();
152        if self.requested {
153            return Ok(());
154        }
155        let Some(state) = input.payload() else {
156            return Ok(());
157        };
158        if state.status != CuRobotStatus::Ready {
159            return Ok(());
160        }
161        self.request.start = state.positions;
162        output.set_payload(self.request.clone());
163        self.requested = true;
164        Ok(())
165    }
166}
167
168#[cfg(test)]
169mod tests {
170    use super::*;
171    use cu29::units::si::angle::radian;
172    use cu29::units::si::f64::Angle;
173
174    fn joint(rad: f64) -> CuJointPosition {
175        CuJointPosition::Revolute(Angle::new::<radian>(rad))
176    }
177
178    fn run(task: &mut CuJointGoalTask<2>, status: CuRobotStatus) -> Option<CuJointPlanRequest<2>> {
179        let state = CuMsg::new(Some(CuJointStatePayload {
180            status,
181            positions: [joint(0.1); 2],
182            ..Default::default()
183        }));
184        let mut output = CuMsg::<CuJointPlanRequest<2>>::default();
185        let (ctx, _clock) = CuContext::new_mock_clock();
186        task.process(&ctx, &state, &mut output).unwrap();
187        output.payload().cloned()
188    }
189
190    #[test]
191    fn test_requests_once_from_first_ready_state() {
192        let mut task =
193            CuJointGoalTask::from_params(CuJointGoalParams::new([joint(0.5); 2])).unwrap();
194        assert!(run(&mut task, CuRobotStatus::NotReady).is_none());
195        let request = run(&mut task, CuRobotStatus::Ready).unwrap();
196        assert_eq!(request.start, [joint(0.1); 2]);
197        assert_eq!(request.goal, [joint(0.5); 2]);
198        assert!(run(&mut task, CuRobotStatus::Ready).is_none());
199    }
200
201    #[test]
202    fn test_reads_typed_goal() {
203        let cfg: ComponentConfig = ron::from_str(
204            r#"#![enable(unwrap_newtypes)] {"goal": [{"revolute": 0.4}, {"revolute": -0.3}]}"#,
205        )
206        .unwrap();
207        let mut task = CuJointGoalTask::<2>::new(Some(&cfg), ()).unwrap();
208        let request = run(&mut task, CuRobotStatus::Ready).unwrap();
209        assert_eq!(request.goal, [joint(0.4), joint(-0.3)]);
210        assert!(CuJointGoalTask::<2>::new(None, ()).is_err());
211        let typo: ComponentConfig = ron::from_str(
212            r#"#![enable(unwrap_newtypes)] {"goal": [{"revolute": 0.4}, {"revolute": -0.3}], "gaol": 1}"#,
213        )
214        .unwrap();
215        assert!(CuJointGoalTask::<2>::new(Some(&typo), ()).is_err());
216    }
217}
repose_motion_planning/goal_task.rs

repose_motion_planning/
goal_task.rs
