341 lines
14 KiB
C++
341 lines
14 KiB
C++
#ifndef CMVR_ES_SEER_ROBOKIT_AGV_H
|
|
#define CMVR_ES_SEER_ROBOKIT_AGV_H
|
|
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <condition_variable>
|
|
#include <cstdint>
|
|
#include <deque>
|
|
#include <mutex>
|
|
#include <string>
|
|
#include <thread>
|
|
#include <vector>
|
|
|
|
#include <json/json.h>
|
|
|
|
#include "cmvr/config/agv_config/agv_config.pb.h"
|
|
#include "devices/agv/abstract_agv.h"
|
|
|
|
namespace cmvr::device {
|
|
|
|
class SeerRobokitAgvTestPeer;
|
|
|
|
class SeerRobokitAgv final : public AbstractAGV {
|
|
public:
|
|
explicit SeerRobokitAgv(const config::SeerRobokitAgvConfig& cfg);
|
|
~SeerRobokitAgv() override;
|
|
|
|
std::string typeName() const override { return "SeerRobokitAgv"; }
|
|
|
|
bool init() override;
|
|
bool start() override;
|
|
bool stop() override;
|
|
bool update() override;
|
|
|
|
AgvRuntimeState runtimeState() const override;
|
|
AgvNavigationStatus navigationStatus() const override;
|
|
|
|
AgvResult emergencyStop() override;
|
|
AgvResult clearFault() override;
|
|
|
|
AgvResult navigateToPose(
|
|
const math::Pose2d& pose,
|
|
const AgvMotionOptions& options = {},
|
|
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
|
|
AgvResult navigateToStation(
|
|
const std::string& station_id,
|
|
const AgvMotionOptions& options = {},
|
|
const AgvAdapterParams& adapter_params = AgvAdapterParams{}) override;
|
|
AgvResult followPath(
|
|
const std::vector<AgvPathSegment>& path) override;
|
|
AgvResult followPath(
|
|
const std::vector<AgvPathSegment>& path,
|
|
const AgvMotionOptions& options) override;
|
|
AgvResult pauseNavigation() override;
|
|
AgvResult resumeNavigation() override;
|
|
AgvResult cancelNavigation() override;
|
|
|
|
AgvResult setVelocity(const AgvVelocity& velocity) override;
|
|
|
|
AgvResult listMaps(std::vector<std::string>& maps) const override;
|
|
AgvResult listStations(std::vector<AgvStation>& stations) const override;
|
|
AgvResult switchMap(const std::string& map_name) override;
|
|
AgvResult uploadMap(const std::string& map_name, const std::string& content) override;
|
|
AgvResult downloadMap(const std::string& map_name, std::string& content) const override;
|
|
AgvResult startMapping(const AgvMappingOptions& options = {}) override;
|
|
AgvResult getMappingData(int start_index, AgvMappingData& data) const override;
|
|
AgvResult getUnifiedMapUpdate(
|
|
std::uint64_t after_sequence,
|
|
const AgvMapStreamOptions& options,
|
|
AgvUnifiedMapUpdate& update) const override;
|
|
AgvResult stopMapping() override;
|
|
|
|
private:
|
|
friend class SeerRobokitAgvTestPeer;
|
|
|
|
struct Ports {
|
|
int status{19204};
|
|
int control{19205};
|
|
int navigation{19206};
|
|
int config{19207};
|
|
int other{19210};
|
|
int push{19301};
|
|
};
|
|
|
|
struct PoseTaskStatus {
|
|
bool found{false};
|
|
int state{0};
|
|
int type{0};
|
|
bool type_present{false};
|
|
double progress{0.0};
|
|
std::string detail;
|
|
};
|
|
|
|
struct NavigationSnapshot {
|
|
int task_status{0};
|
|
int task_type{0};
|
|
bool task_status_present{false};
|
|
bool task_type_present{false};
|
|
bool blocked{false};
|
|
bool blocked_present{false};
|
|
int block_reason{-1};
|
|
std::string block_reason_raw;
|
|
bool velocity_present{false};
|
|
double vx{0.0};
|
|
double vy{0.0};
|
|
double w{0.0};
|
|
bool emergency{false};
|
|
std::string target_id;
|
|
std::string active_faults;
|
|
std::string detail;
|
|
};
|
|
|
|
enum class CommandTransmissionState {
|
|
NotSent,
|
|
PossiblySent,
|
|
};
|
|
|
|
struct TrackedNavigationContext {
|
|
std::string token;
|
|
std::vector<std::string> task_ids;
|
|
AgvTaskType type{AgvTaskType::None};
|
|
std::string target_id;
|
|
std::vector<std::string> target_ids;
|
|
std::uint64_t navigation_generation{0};
|
|
std::chrono::steady_clock::time_point accepted_at{};
|
|
bool synchronous_wait{false};
|
|
};
|
|
|
|
struct PoseTaskContext {
|
|
std::string task_id;
|
|
math::Pose2d target{};
|
|
double reach_distance{0.0};
|
|
double reach_angle{0.0};
|
|
std::uint64_t navigation_generation{0};
|
|
std::uint64_t controller_fault_sequence_at_start{0};
|
|
std::uint64_t control_attempt_sequence_at_start{0};
|
|
std::uint64_t controller_fault_channel_epoch_at_start{0};
|
|
};
|
|
|
|
AgvResult connect_();
|
|
AgvResult disconnect_();
|
|
AgvResult emergencyStopTrackedNavigation_(
|
|
const TrackedNavigationContext* expected_navigation);
|
|
AgvResult connectSocket_(int& sock, int port);
|
|
AgvResult ensureOtherSocket_();
|
|
void closeSocket_(int& sock) const;
|
|
bool connected_() const;
|
|
|
|
AgvResult acquireControl_() const;
|
|
AgvResult confirmPoseNavigationStarted_(
|
|
const PoseTaskContext& context,
|
|
bool accept_paused,
|
|
const AgvMotionOptions& options) const;
|
|
AgvResult waitForPoseNavigationTerminal_(
|
|
const PoseTaskContext& pose_context,
|
|
const TrackedNavigationContext& navigation_context,
|
|
const AgvMotionOptions& options);
|
|
AgvResult waitForTrackedNavigationTerminal_(
|
|
const TrackedNavigationContext& context,
|
|
const AgvMotionOptions& options);
|
|
AgvResult queryNavigationSnapshot_(NavigationSnapshot& snapshot) const;
|
|
AgvResult cancelTrackedNavigation_(
|
|
const TrackedNavigationContext& context,
|
|
std::uint64_t& accepted_generation);
|
|
AgvResult waitForCanceledTaskToStop_(
|
|
const TrackedNavigationContext& context,
|
|
const AgvMotionOptions& options,
|
|
const std::string& reason);
|
|
AgvResult failAndCancelTrackedNavigation_(
|
|
const TrackedNavigationContext& context,
|
|
const AgvMotionOptions& options,
|
|
AgvErrorCode error_code,
|
|
const std::string& reason);
|
|
AgvResult queryPoseTaskStatus_(
|
|
const std::string& task_id,
|
|
PoseTaskStatus& status) const;
|
|
AgvResult queryTaskStatuses_(
|
|
const std::vector<std::string>& task_ids,
|
|
std::vector<PoseTaskStatus>& statuses) const;
|
|
bool poseTargetReached_(
|
|
const PoseTaskContext& context,
|
|
std::string& detail) const;
|
|
std::string cachedControllerFaultDetail_(
|
|
std::uint64_t after_sequence = 0,
|
|
int wait_ms = 0,
|
|
std::uint64_t* associated_control_attempt = nullptr) const;
|
|
int controllerFaultCaptureGraceMs_() const;
|
|
int controllerFaultStateMaxAgeMs_() const;
|
|
std::string freeNavigationFaultStateUnavailableDetail_() const;
|
|
void rememberPoseTask_(const PoseTaskContext& context) const;
|
|
void advancePoseTaskGeneration_(
|
|
std::uint64_t navigation_generation,
|
|
std::uint64_t control_attempt_sequence) const;
|
|
void advancePoseTaskControlAttempt_(
|
|
std::uint64_t control_attempt_sequence) const;
|
|
void clearPoseTask_(std::uint64_t navigation_generation) const;
|
|
void clearPoseTaskIfTaskId_(const std::string& task_id) const;
|
|
bool currentPoseTask_(PoseTaskContext& context) const;
|
|
void rememberTrackedNavigation_(
|
|
const TrackedNavigationContext& context) const;
|
|
void advanceTrackedNavigationGeneration_(
|
|
std::uint64_t navigation_generation) const;
|
|
void clearTrackedNavigation_(std::uint64_t navigation_generation) const;
|
|
void clearTrackedNavigationIfToken_(const std::string& token) const;
|
|
bool currentTrackedNavigation_(
|
|
TrackedNavigationContext& context) const;
|
|
AgvResult sendControlledCommand_(int sock,
|
|
std::uint16_t command,
|
|
const Json::Value& payload,
|
|
Json::Value* response,
|
|
std::uint64_t* accepted_navigation_generation = nullptr,
|
|
std::uint64_t* controller_fault_sequence_at_attempt = nullptr,
|
|
std::uint64_t* control_attempt_sequence = nullptr,
|
|
PoseTaskContext* pose_context_to_publish = nullptr,
|
|
bool reject_if_active_controller_fault = false,
|
|
TrackedNavigationContext* navigation_context_to_publish = nullptr,
|
|
bool preserve_tracked_navigation = false,
|
|
const std::string* expected_navigation_token = nullptr,
|
|
const std::function<bool()>* cancellation_requested = nullptr,
|
|
const TrackedNavigationContext* expected_active_navigation = nullptr) const;
|
|
AgvResult sendCommand_(int sock,
|
|
std::uint16_t command,
|
|
const Json::Value& payload,
|
|
Json::Value* response,
|
|
CommandTransmissionState* transmission_state = nullptr) const;
|
|
AgvResult sendCommandRaw_(int sock,
|
|
std::uint16_t command,
|
|
const Json::Value& payload,
|
|
std::string* response_payload,
|
|
CommandTransmissionState* transmission_state = nullptr) const;
|
|
AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) const;
|
|
AgvResult configurePush_();
|
|
void startPushThread_();
|
|
void stopPushThread_();
|
|
void pushLoop_();
|
|
void invalidateControllerFaultState_();
|
|
AgvRuntimeState queryRuntimeState_() const;
|
|
void updateCachedRuntimeState_(const Json::Value& payload);
|
|
void startMapUpdateThread_();
|
|
void stopMapUpdateThread_();
|
|
void mapUpdateLoop_();
|
|
AgvResult refreshMapCacheOnce_(const AgvMapStreamOptions& options) const;
|
|
AgvResult parseMapFileToUpdates_(
|
|
const std::string& file_name,
|
|
const std::string& content,
|
|
const AgvMapStreamOptions& options,
|
|
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
|
AgvResult parseSeerRobokitMapArchive_(
|
|
const std::string& file_name,
|
|
const std::string& content,
|
|
const AgvMapStreamOptions& options,
|
|
std::vector<AgvUnifiedMapUpdate>& updates) const;
|
|
AgvResult parseSeerRobokitMap2D_(
|
|
const std::string& file_name,
|
|
const std::string& content,
|
|
const AgvMapStreamOptions& options,
|
|
AgvUnifiedMapUpdate& update) const;
|
|
AgvResult parseSeerRobokitMap3D_(
|
|
const std::string& file_name,
|
|
const std::string& content,
|
|
const AgvMapStreamOptions& options,
|
|
AgvUnifiedMapUpdate& update) const;
|
|
void cacheMapUpdates_(std::vector<AgvUnifiedMapUpdate> updates) const;
|
|
bool findCachedMapUpdate_(
|
|
std::uint64_t after_sequence,
|
|
const AgvMapStreamOptions& options,
|
|
AgvUnifiedMapUpdate& update) const;
|
|
bool mapUpdateMatches_(
|
|
const AgvUnifiedMapUpdate& update,
|
|
const AgvMapStreamOptions& options) const;
|
|
|
|
static std::vector<std::uint8_t> buildFrame_(std::uint16_t command, const std::string& payload);
|
|
static std::string toJsonString_(const Json::Value& value);
|
|
static bool parseJson_(const std::string& input, Json::Value& output, std::string& error);
|
|
static std::string extractJson_(const std::string& raw);
|
|
static AgvResult receiveFrame_(int sock, std::uint16_t& command, std::string& payload);
|
|
static void applyMotionOptions_(
|
|
Json::Value& payload,
|
|
const AgvMotionOptions& options,
|
|
bool include_reach_options = true);
|
|
static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
|
|
static AgvResult resultFromResponse_(const Json::Value& response);
|
|
|
|
config::SeerRobokitAgvConfig config_;
|
|
std::string ip_;
|
|
std::string control_nick_name_;
|
|
int recv_timeout_ms_{1000};
|
|
Ports ports_;
|
|
bool state_push_enabled_{false};
|
|
bool map_update_enabled_{false};
|
|
int map_update_interval_ms_{1000};
|
|
std::size_t map_update_history_size_{8};
|
|
|
|
mutable std::mutex mutex_;
|
|
mutable std::mutex status_io_mutex_;
|
|
mutable std::mutex control_sequence_mutex_;
|
|
mutable std::atomic<std::uint64_t> navigation_generation_{0};
|
|
mutable std::atomic<std::uint64_t> pose_task_sequence_{0};
|
|
mutable std::atomic<std::uint64_t> control_attempt_sequence_{0};
|
|
mutable std::atomic<std::uint64_t> controller_fault_channel_epoch_{0};
|
|
mutable std::mutex pose_task_mutex_;
|
|
mutable PoseTaskContext pose_task_context_;
|
|
mutable std::mutex tracked_navigation_mutex_;
|
|
mutable TrackedNavigationContext tracked_navigation_context_;
|
|
mutable int sock_status_{-1};
|
|
mutable int sock_control_{-1};
|
|
mutable int sock_navigation_{-1};
|
|
mutable int sock_config_{-1};
|
|
mutable int sock_other_{-1};
|
|
mutable int sock_push_{-1};
|
|
std::string last_error_;
|
|
|
|
std::atomic<bool> push_running_{false};
|
|
std::thread push_thread_;
|
|
mutable std::mutex runtime_state_mutex_;
|
|
mutable std::condition_variable runtime_state_cv_;
|
|
AgvRuntimeState cached_runtime_state_;
|
|
bool cached_runtime_state_valid_{false};
|
|
std::uint64_t controller_fault_sequence_{0};
|
|
bool controller_fault_state_observed_{false};
|
|
std::chrono::steady_clock::time_point controller_fault_state_observed_at_{};
|
|
std::string active_controller_fault_detail_;
|
|
double last_controller_fault_timestamp_{0.0};
|
|
std::string last_controller_fault_detail_;
|
|
std::uint64_t last_controller_fault_control_attempt_{0};
|
|
|
|
mutable std::atomic<bool> map_update_running_{false};
|
|
mutable std::thread map_update_thread_;
|
|
mutable std::mutex map_update_mutex_;
|
|
mutable std::condition_variable map_update_cv_;
|
|
mutable std::deque<AgvUnifiedMapUpdate> cached_map_updates_;
|
|
mutable std::uint64_t map_sequence_{0};
|
|
mutable int next_mapping_index_{0};
|
|
mutable std::size_t last_map_content_hash_{0};
|
|
mutable std::string map_session_id_;
|
|
};
|
|
|
|
} // namespace cmvr::device
|
|
|
|
#endif // CMVR_ES_SEER_ROBOKIT_AGV_H
|