104 lines
3.6 KiB
C++
104 lines
3.6 KiB
C++
#ifndef CMVR_ES_SRC1100_AGV_H
|
|
#define CMVR_ES_SRC1100_AGV_H
|
|
|
|
#include <mutex>
|
|
#include <string>
|
|
#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 Src1100Agv final : public AbstractAGV {
|
|
public:
|
|
explicit Src1100Agv(const config::Src1100AgvConfig& cfg);
|
|
~Src1100Agv() override;
|
|
|
|
std::string typeName() const override { return "Src1100Agv"; }
|
|
|
|
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 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;
|
|
|
|
private:
|
|
struct Ports {
|
|
int status{19204};
|
|
int control{19205};
|
|
int navigation{19206};
|
|
int config{19207};
|
|
int other{19210};
|
|
int push{19301};
|
|
};
|
|
|
|
AgvResult connect_();
|
|
AgvResult disconnect_();
|
|
AgvResult connectSocket_(int& sock, int port);
|
|
void closeSocket_(int& sock) const;
|
|
bool connected_() const;
|
|
|
|
AgvResult sendCommand_(int sock,
|
|
std::uint16_t command,
|
|
const Json::Value& payload,
|
|
Json::Value* response) const;
|
|
AgvResult sendCommandNoResponse_(int sock, std::uint16_t command, const Json::Value& payload) 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 int optionalInt_(const AgvAdapterParams& params, const std::string& key, int fallback);
|
|
static double optionalDouble_(const AgvAdapterParams& params, const std::string& key, double fallback);
|
|
static void applyMotionOptions_(Json::Value& payload, const AgvMotionOptions& options);
|
|
static void applyAdapterParams_(Json::Value& payload, const AgvAdapterParams& params);
|
|
static AgvResult resultFromResponse_(const Json::Value& response);
|
|
|
|
config::Src1100AgvConfig config_;
|
|
std::string ip_;
|
|
int recv_timeout_ms_{1000};
|
|
Ports ports_;
|
|
|
|
mutable std::mutex mutex_;
|
|
int sock_status_{-1};
|
|
int sock_control_{-1};
|
|
int sock_navigation_{-1};
|
|
int sock_config_{-1};
|
|
int sock_other_{-1};
|
|
int sock_push_{-1};
|
|
std::string last_error_;
|
|
};
|
|
|
|
} // namespace cmvr::device
|
|
|
|
#endif // CMVR_ES_SRC1100_AGV_H
|