oxictl 0.1.1

Pure Rust Real-Time Control Systems Framework
Documentation 
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

//! ROS2 action layer — type-safe goal/feedback/result on top of DDS services.

pub mod client;
pub mod server;
pub mod types;

pub use client::ActionClient;
pub use server::ActionServer;
use server::ActionServerParts;
pub use types::{
    Action, ActionHandler, ExecuteResult, GetResultRequest, GetResultResponse, GetResultService,
    GoalOutcome, SendGoalRequest, SendGoalResponse, SendGoalService,
};

use heapless::String as HString;

use crate::protocol::dds::api::error::DdsApiError;
use crate::protocol::dds::api::participant::Participant;
use crate::protocol::dds::api::service::{
    create_client as create_svc_client, ReplyWrapper, RequestWrapper,
};
use crate::protocol::dds::discovery::qos_profile::QosProfile;
use crate::protocol::dds::ros2::msgs::action_msgs::{CancelGoal, GoalStatusArray};
use crate::protocol::dds::ros2::topic_naming::{encode_topic_name, Ros2TopicKind};

fn action_sub_name(action_name: &str, suffix: &str) -> Result<HString<256>, DdsApiError> {
    let mut s: HString<256> = HString::new();
    s.push_str(action_name)
        .map_err(|_| DdsApiError::Serialization("action name too long"))?;
    s.push_str(suffix)
        .map_err(|_| DdsApiError::Serialization("action name suffix too long"))?;
    Ok(s)
}

fn encode_action_topic(topic_name: &str, kind: Ros2TopicKind) -> Result<HString<256>, DdsApiError> {
    let mut encoded: HString<256> = HString::new();
    encode_topic_name(&mut encoded, topic_name, kind).map_err(|_| DdsApiError::TopicNameTooLong)?;
    Ok(encoded)
}

/// Create an [`ActionServer`] for action `action_name`, wiring up all six
/// request/reply service pairs plus the feedback and status publishers.
pub fn create_action_server<A: Action>(
    participant: &mut Participant,
    action_name: &str,
    qos: &QosProfile,
) -> Result<ActionServer<A>, DdsApiError> {
    // SendGoal
    let sg_name = action_sub_name(action_name, "/_action/send_goal")?;
    let sg_req_topic = encode_action_topic(sg_name.as_str(), Ros2TopicKind::ServiceRequest)?;
    let sg_rep_topic = encode_action_topic(sg_name.as_str(), Ros2TopicKind::ServiceReply)?;
    let send_goal_req_sub = participant
        .create_subscription::<RequestWrapper<SendGoalService<A>>>(sg_req_topic.as_str(), qos)?;
    let send_goal_rep_pub = participant
        .create_publisher::<ReplyWrapper<SendGoalService<A>>>(sg_rep_topic.as_str(), qos)?;

    // CancelGoal
    let cg_name = action_sub_name(action_name, "/_action/cancel_goal")?;
    let cg_req_topic = encode_action_topic(cg_name.as_str(), Ros2TopicKind::ServiceRequest)?;
    let cg_rep_topic = encode_action_topic(cg_name.as_str(), Ros2TopicKind::ServiceReply)?;
    let cancel_goal_req_sub = participant
        .create_subscription::<RequestWrapper<CancelGoal>>(cg_req_topic.as_str(), qos)?;
    let cancel_goal_rep_pub =
        participant.create_publisher::<ReplyWrapper<CancelGoal>>(cg_rep_topic.as_str(), qos)?;

    // GetResult
    let gr_name = action_sub_name(action_name, "/_action/get_result")?;
    let gr_req_topic = encode_action_topic(gr_name.as_str(), Ros2TopicKind::ServiceRequest)?;
    let gr_rep_topic = encode_action_topic(gr_name.as_str(), Ros2TopicKind::ServiceReply)?;
    let get_result_req_sub = participant
        .create_subscription::<RequestWrapper<GetResultService<A>>>(gr_req_topic.as_str(), qos)?;
    let get_result_rep_pub = participant
        .create_publisher::<ReplyWrapper<GetResultService<A>>>(gr_rep_topic.as_str(), qos)?;

    // Feedback publisher
    let fb_name = action_sub_name(action_name, "/_action/feedback")?;
    let fb_topic = encode_action_topic(fb_name.as_str(), Ros2TopicKind::Topic)?;
    let feedback_pub = participant.create_publisher::<A::Feedback>(fb_topic.as_str(), qos)?;

    // Status publisher
    let st_name = action_sub_name(action_name, "/_action/status")?;
    let st_topic = encode_action_topic(st_name.as_str(), Ros2TopicKind::Topic)?;
    let status_pub = participant.create_publisher::<GoalStatusArray>(st_topic.as_str(), qos)?;

    Ok(ActionServer::new(ActionServerParts {
        send_goal_req_sub,
        send_goal_rep_pub,
        cancel_goal_req_sub,
        cancel_goal_rep_pub,
        get_result_req_sub,
        get_result_rep_pub,
        feedback_pub,
        status_pub,
    }))
}

/// Create an [`ActionClient`] for action `action_name`, wiring up all three
/// service clients plus the feedback and status subscriptions.
pub fn create_action_client<A: Action>(
    participant: &mut Participant,
    action_name: &str,
    qos: &QosProfile,
) -> Result<ActionClient<A>, DdsApiError> {
    let sg_name = action_sub_name(action_name, "/_action/send_goal")?;
    let send_goal_client =
        create_svc_client::<SendGoalService<A>>(participant, sg_name.as_str(), qos)?;

    let gr_name = action_sub_name(action_name, "/_action/get_result")?;
    let get_result_client =
        create_svc_client::<GetResultService<A>>(participant, gr_name.as_str(), qos)?;

    let cg_name = action_sub_name(action_name, "/_action/cancel_goal")?;
    let cancel_client = create_svc_client::<CancelGoal>(participant, cg_name.as_str(), qos)?;

    // Feedback subscription
    let fb_name = action_sub_name(action_name, "/_action/feedback")?;
    let fb_topic = encode_action_topic(fb_name.as_str(), Ros2TopicKind::Topic)?;
    let feedback_sub = participant.create_subscription::<A::Feedback>(fb_topic.as_str(), qos)?;

    // Status subscription
    let st_name = action_sub_name(action_name, "/_action/status")?;
    let st_topic = encode_action_topic(st_name.as_str(), Ros2TopicKind::Topic)?;
    let status_sub = participant.create_subscription::<GoalStatusArray>(st_topic.as_str(), qos)?;

    Ok(ActionClient::new(
        send_goal_client,
        get_result_client,
        cancel_client,
        feedback_sub,
        status_sub,
    ))
}