kos_core 0.1.2

The K-Scale Operating System
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

use crate::grpc_interface::google::longrunning::Operation;
use crate::hal::Actuator;
use crate::kos_proto::actuator::actuator_service_server::ActuatorService;
use crate::kos_proto::actuator::*;
use crate::kos_proto::common::ActionResponse;
use crate::telemetry::Telemetry;
use crate::telemetry_types::{ActuatorCommand, ActuatorState};
use std::sync::Arc;
use tonic::{Request, Response, Status};
use tracing::trace;

pub struct ActuatorServiceImpl {
    actuator: Arc<dyn Actuator>,
}

impl ActuatorServiceImpl {
    pub fn new(actuator: Arc<dyn Actuator>) -> Self {
        Self { actuator }
    }
}

#[tonic::async_trait]
impl ActuatorService for ActuatorServiceImpl {
    async fn command_actuators(
        &self,
        request: Request<CommandActuatorsRequest>,
    ) -> Result<Response<CommandActuatorsResponse>, Status> {
        let commands = request.into_inner().commands;

        let telemetry_commands: Vec<_> = commands.iter().map(ActuatorCommand::from).collect();

        let results = self
            .actuator
            .command_actuators(commands)
            .await
            .map_err(|e| Status::internal(format!("Failed to command actuators, {:?}", e)))?;

        trace!(
            "Commanding actuators, request: {:?}, results: {:?}",
            telemetry_commands.clone(),
            results
        );

        let telemetry = Telemetry::get().await;
        if let Some(telemetry) = telemetry {
            telemetry.increment_inference_step();

            if let Err(e) = telemetry
                .publish("actuator/command", &telemetry_commands)
                .await
            {
                tracing::warn!("Failed to publish telemetry: {}", e);
            }
        }

        Ok(Response::new(CommandActuatorsResponse { results }))
    }

    async fn configure_actuator(
        &self,
        request: Request<ConfigureActuatorRequest>,
    ) -> Result<Response<ActionResponse>, Status> {
        let config = request.into_inner();
        let response = self
            .actuator
            .configure_actuator(config)
            .await
            .map_err(|e| Status::internal(format!("Failed to configure actuator, {:?}", e)))?;
        Ok(Response::new(response))
    }

    async fn calibrate_actuator(
        &self,
        request: Request<CalibrateActuatorRequest>,
    ) -> Result<Response<Operation>, Status> {
        let calibrate_request = request.into_inner();
        let operation = self
            .actuator
            .calibrate_actuator(calibrate_request)
            .await
            .map_err(|e| Status::internal(format!("Failed to calibrate actuator, {:?}", e)))?;

        Ok(Response::new(operation))
    }

    async fn get_actuators_state(
        &self,
        request: Request<GetActuatorsStateRequest>,
    ) -> Result<Response<GetActuatorsStateResponse>, Status> {
        let request = request.into_inner();
        let actuator_ids = request.actuator_ids.clone();
        let states = self
            .actuator
            .get_actuators_state(actuator_ids)
            .await
            .map_err(|e| Status::internal(format!("Failed to get actuators state, {:?}", e)))?;

        let telemetry_states: Vec<_> = states.iter().map(ActuatorState::from).collect();
        let telemetry = Telemetry::get().await;
        if let Some(telemetry) = telemetry {
            if let Err(e) = telemetry.publish("actuator/state", &telemetry_states).await {
                tracing::warn!("Failed to publish telemetry: {}", e);
            }
        }

        trace!(
            "Getting actuators state, request: {:?}, response: {:?}",
            request.clone(),
            states
        );
        Ok(Response::new(GetActuatorsStateResponse { states }))
    }
}