mt_action 0.9.0

Action goal lifecycle primitives for Minot.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164

use anyhow::anyhow;
use log::{error, info, warn};
use mt_action::ActionClient;
use mt_pubsub::{Node, NodeConfig, Qos};
use ros2_interfaces_jazzy_rkyv::geometry_msgs;
use ros2_interfaces_jazzy_rkyv::std_msgs;
use std::sync::Arc;
use tokio::time::Duration;

use mt_action::messages::*;

#[tokio::main]
async fn main() -> anyhow::Result<()> {
    mt_sea::init_logging();

    let node = Arc::new(Node::create(NodeConfig::new("robot_client")).await?);

    let client: Arc<
        ActionClient<
            std_msgs::msg::String,
            (u64, geometry_msgs::msg::Twist),
            std_msgs::msg::String,
        >,
    > = ActionClient::new(node.clone(), "/robot_action".to_owned(), Qos::Reliable).await?;

    if !ActionClient::wait_for_action_server(client.clone(), Duration::from_secs(5)).await {
        return Err(anyhow!("Action server not ready after 5s"));
    }

    info!("Action server ready");

    // goals to perform for the provided amount of milliseconds
    // time doesn't matter here and is handled by server
    // when actually perfoming the goal
    // (id, (millis, twist))
    let goals = [
        (
            0,
            (
                3_000,
                geometry_msgs::msg::Twist {
                    linear: geometry_msgs::msg::Vector3 {
                        x: 0.1,
                        y: 0.0,
                        z: 0.0,
                    },
                    angular: geometry_msgs::msg::Vector3 {
                        x: 0.0,
                        y: 0.0,
                        z: 0.0,
                    },
                },
            ),
        ),
        (
            1,
            (
                10_000,
                geometry_msgs::msg::Twist {
                    linear: geometry_msgs::msg::Vector3 {
                        x: 0.0,
                        y: 0.0,
                        z: 0.0,
                    },
                    angular: geometry_msgs::msg::Vector3 {
                        x: 0.0,
                        y: 0.0,
                        z: 0.2,
                    },
                },
            ),
        ),
        (
            2,
            (
                5_000,
                geometry_msgs::msg::Twist {
                    linear: geometry_msgs::msg::Vector3 {
                        x: -0.1,
                        y: 0.0,
                        z: 0.0,
                    },
                    angular: geometry_msgs::msg::Vector3 {
                        x: 0.0,
                        y: 0.0,
                        z: 0.0,
                    },
                },
            ),
        ),
    ];

    let status_client = client.clone();
    let status_handle = tokio::spawn(async move {
        while let Ok(status) = ActionClient::get_status(status_client.clone()).await {
            info!("Got status update: {:?}", status);
        }
    });

    let feedback_client = client.clone();
    let feedback_handle = tokio::spawn(async move {
        while let Ok(feedback) = ActionClient::get_feedback(feedback_client.clone()).await {
            info!(
                "Got feedback update for {}: {}",
                feedback.id, feedback.msg.data
            );
        }
    });

    info!("Sending some goal request");
    for goal in goals.iter() {
        info!("Sending: {:?}", &goal);
        match ActionClient::send_goal(
            client.clone(),
            ActionSendGoalRequest {
                id: goal.0,
                goal: goal.1.clone(),
            },
            None,
        )
        .await
        {
            Ok(res) => {
                if res.accepted {
                    info!("{:?}", res);
                } else {
                    error!("{:?}", res);
                }
            }
            Err(e) => {
                error!("{:?}", e);
            }
        }
    }

    info!("Waiting for the results");
    for goal in goals.iter() {
        info!("Requesting: {:?}", &goal.0);
        match ActionClient::get_result(client.clone(), ActionGetResultRequest { id: goal.0 }, None)
            .await
        {
            Ok(res) => match res.status {
                GoalState::Aborted => {
                    error!("{:?}", res);
                }
                GoalState::Canceled => {
                    warn!("{:?}", res);
                }
                _ => {
                    info!("{:?}", res);
                }
            },
            Err(e) => {
                error!("{:?}", e);
            }
        }
    }

    // we don't need these anymore
    status_handle.abort();
    feedback_handle.abort();

    Ok(())
}