[A]add sdk_core.
This commit is contained in:
Livox-SDK
@@ -0,0 +1,242 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "command_channel.h"
|
||||
#include <boost/bind.hpp>
|
||||
#include "base/logging.h"
|
||||
#include "base/network_util.h"
|
||||
#include "command_impl.h"
|
||||
#include "device_manager.h"
|
||||
#include "livox_def.h"
|
||||
|
||||
using boost::atomic_uint16_t;
|
||||
using boost::bind;
|
||||
using std::list;
|
||||
using std::make_pair;
|
||||
using std::map;
|
||||
using std::pair;
|
||||
using std::string;
|
||||
|
||||
namespace livox {
|
||||
|
||||
CommandChannel::CommandChannel(apr_port_t port,
|
||||
uint8_t handle,
|
||||
const string &remote_ip,
|
||||
CommandChannelDelegate *cb,
|
||||
apr_pool_t *pool)
|
||||
: handle_(handle),
|
||||
port_(port),
|
||||
sock_(NULL),
|
||||
mem_pool_(pool),
|
||||
loop_(NULL),
|
||||
callback_(cb),
|
||||
comm_port_(new CommPort),
|
||||
heartbeat_time_(0),
|
||||
remote_ip_(remote_ip),
|
||||
last_heartbeat_(0) {}
|
||||
|
||||
bool CommandChannel::Bind(IOLoop *loop) {
|
||||
if (loop == NULL) {
|
||||
return false;
|
||||
}
|
||||
loop_ = loop;
|
||||
sock_ = util::CreateBindSocket(port_, mem_pool_);
|
||||
if (sock_ == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
loop_->AddDelegate(sock_, this);
|
||||
return true;
|
||||
}
|
||||
|
||||
void CommandChannel::OnData(apr_socket_t *, void *) {
|
||||
apr_sockaddr_t addr;
|
||||
uint32_t buf_size = 0;
|
||||
uint8_t *cache_buf = comm_port_->FetchCacheFreeSpace(&buf_size);
|
||||
apr_size_t size = buf_size;
|
||||
apr_status_t rv = apr_socket_recvfrom(&addr, sock_, 0, reinterpret_cast<char *>(cache_buf), &size);
|
||||
comm_port_->UpdateCacheWrIdx(size);
|
||||
if (rv != APR_SUCCESS) {
|
||||
LOG_ERROR << PrintAPRStatus(rv) << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
CommPacket packet;
|
||||
memset(&packet, 0, sizeof(packet));
|
||||
|
||||
while ((kParseSuccess == comm_port_->ParseCommStream(&packet))) {
|
||||
if (packet.packet_type == kCommandTypeAck) {
|
||||
uint16_t seq = packet.seq_num;
|
||||
if (commands_.find(seq) != commands_.end()) {
|
||||
Command command = commands_[seq].first;
|
||||
command.packet = packet;
|
||||
if (callback_) {
|
||||
callback_->OnCommand(handle_, command);
|
||||
}
|
||||
commands_.erase(seq);
|
||||
} else if (packet.cmd_set == kCommandSetGeneral && packet.cmd_code == kCommandIDGeneralHeartbeat) {
|
||||
OnHeartbeatAck(packet);
|
||||
if (callback_) {
|
||||
callback_->OnHeartbeatStateUpdate(handle_, *(reinterpret_cast<HeartbeatResponse *>(packet.data)));
|
||||
}
|
||||
}
|
||||
} else if (packet.packet_type == kCommandTypeMsg) {
|
||||
if (callback_) {
|
||||
Command command;
|
||||
command.packet = packet;
|
||||
callback_->OnCommand(handle_, command);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CommandChannel::SendAsync(const Command &command) {
|
||||
Command cmd = DeepCopy(command);
|
||||
if (loop_) {
|
||||
loop_->PostTask(bind(&CommandChannel::Send, this, cmd));
|
||||
}
|
||||
}
|
||||
|
||||
void CommandChannel::OnTimer(apr_time_t now) {
|
||||
list<Command> timeout_commands;
|
||||
map<uint16_t, pair<Command, apr_time_t> >::iterator ite = commands_.begin();
|
||||
while (ite != commands_.end()) {
|
||||
pair<Command, apr_time_t> &command_pair = ite->second;
|
||||
if (now > command_pair.second) {
|
||||
timeout_commands.push_back(command_pair.first);
|
||||
commands_.erase(ite++);
|
||||
} else {
|
||||
++ite;
|
||||
}
|
||||
}
|
||||
|
||||
for (list<Command>::iterator ite = timeout_commands.begin(); ite != timeout_commands.end(); ++ite) {
|
||||
LOG_WARN << PrintAPRTime(apr_time_now()) << " Command Timeout: Set " << (uint16_t)ite->packet.cmd_set << " Id "
|
||||
<< ite->packet.cmd_code << " Seq " << ite->packet.seq_num << std::endl;
|
||||
if (callback_) {
|
||||
ite->packet.packet_type = kCommandTypeAck;
|
||||
callback_->OnCommand(handle_, *ite);
|
||||
}
|
||||
}
|
||||
|
||||
if ((last_heartbeat_ != 0) && (now - last_heartbeat_ > apr_time_from_sec(3))) {
|
||||
DeviceDisconnect(handle_);
|
||||
} else {
|
||||
HeartBeat(now);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandChannel::Uninit() {
|
||||
if (sock_) {
|
||||
loop_->RemoveDelegate(sock_, this);
|
||||
loop_ = NULL;
|
||||
apr_socket_close(sock_);
|
||||
sock_ = NULL;
|
||||
}
|
||||
callback_ = NULL;
|
||||
if (comm_port_) {
|
||||
comm_port_.reset(NULL);
|
||||
}
|
||||
|
||||
commands_.clear();
|
||||
last_heartbeat_ = 0;
|
||||
heartbeat_time_ = 0;
|
||||
remote_ip_ = "";
|
||||
}
|
||||
|
||||
void CommandChannel::HeartBeat(apr_time_t t) {
|
||||
if (heartbeat_time_ == 0 || (t - heartbeat_time_) > apr_time_from_msec(kHeartbeatTimer)) {
|
||||
heartbeat_time_ = t;
|
||||
|
||||
Command command(handle_,
|
||||
kCommandTypeCmd,
|
||||
kCommandSetGeneral,
|
||||
kCommandIDGeneralHeartbeat,
|
||||
GenerateSeq(),
|
||||
NULL,
|
||||
0,
|
||||
0,
|
||||
boost::shared_ptr<CommandCallback>());
|
||||
SendInternal(command);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandChannel::SendInternal(const Command &command) {
|
||||
apr_pool_t *subpool = NULL;
|
||||
apr_pool_create(&subpool, mem_pool_);
|
||||
uint8_t *buf = (uint8_t *)apr_palloc(subpool, kMaxCommandBufferSize);
|
||||
uint32_t size = 0;
|
||||
comm_port_->Pack(buf, kMaxCommandBufferSize, &size, command.packet);
|
||||
apr_size_t apr_size = size;
|
||||
apr_status_t rv = APR_SUCCESS;
|
||||
apr_sockaddr_t *sa = NULL;
|
||||
rv = apr_sockaddr_info_get(&sa, remote_ip_.c_str(), APR_INET, 65000, 0, subpool);
|
||||
if (rv == APR_SUCCESS) {
|
||||
rv = apr_socket_sendto(sock_, sa, 0, (const char *)buf, &apr_size);
|
||||
}
|
||||
apr_pool_destroy(subpool);
|
||||
}
|
||||
|
||||
uint16_t CommandChannel::GenerateSeq() {
|
||||
static atomic_uint16_t seq(1);
|
||||
uint16_t value = seq.load();
|
||||
uint16_t desired = 0;
|
||||
do {
|
||||
if (value == UINT16_MAX) {
|
||||
desired = 1;
|
||||
} else {
|
||||
desired = value + 1;
|
||||
}
|
||||
} while (!seq.compare_exchange_weak(value, desired));
|
||||
return desired;
|
||||
}
|
||||
|
||||
Command CommandChannel::DeepCopy(const Command &cmd) {
|
||||
Command result_cmd(cmd);
|
||||
if (result_cmd.packet.data != NULL) {
|
||||
result_cmd.packet.data = new uint8_t[result_cmd.packet.data_len];
|
||||
memcpy(result_cmd.packet.data, cmd.packet.data, result_cmd.packet.data_len);
|
||||
}
|
||||
return result_cmd;
|
||||
}
|
||||
|
||||
void CommandChannel::OnHeartbeatAck(const CommPacket &) {
|
||||
last_heartbeat_ = apr_time_now();
|
||||
}
|
||||
|
||||
void CommandChannel::DeviceDisconnect(uint8_t handle) {
|
||||
DeviceRemove(handle);
|
||||
}
|
||||
|
||||
void CommandChannel::Send(const Command &command) {
|
||||
SendInternal(command);
|
||||
commands_[command.packet.seq_num] = make_pair(command, apr_time_now() + apr_time_from_msec(command.time_out));
|
||||
Command &cmd = commands_[command.packet.seq_num].first;
|
||||
if (cmd.packet.data != NULL) {
|
||||
delete cmd.packet.data;
|
||||
cmd.packet.data = NULL;
|
||||
cmd.packet.data_len = 0;
|
||||
}
|
||||
}
|
||||
} // namespace livox
|
||||
@@ -0,0 +1,126 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef LIVOX_COMMAND_CHANNEL_H_
|
||||
#define LIVOX_COMMAND_CHANNEL_H_
|
||||
#include <boost/smart_ptr.hpp>
|
||||
#include <list>
|
||||
#include <string>
|
||||
#include "apr_network_io.h"
|
||||
#include "base/io_loop.h"
|
||||
#include "comm/comm_port.h"
|
||||
|
||||
namespace livox {
|
||||
|
||||
typedef struct TagCommand {
|
||||
uint8_t handle;
|
||||
CommPacket packet;
|
||||
boost::shared_ptr<CommandCallback> cb;
|
||||
uint32_t time_out;
|
||||
TagCommand() : packet(), time_out(0) {}
|
||||
TagCommand(uint8_t _handle,
|
||||
uint8_t _cmd_type,
|
||||
uint8_t _cmd_set,
|
||||
uint8_t _cmd_code,
|
||||
uint16_t _seq_num,
|
||||
uint8_t *data,
|
||||
uint16_t length,
|
||||
uint32_t _time_out,
|
||||
const boost::shared_ptr<CommandCallback> &_cb)
|
||||
: handle(_handle), packet(), cb(_cb) {
|
||||
packet.packet_type = _cmd_type;
|
||||
packet.cmd_set = _cmd_set;
|
||||
packet.cmd_code = _cmd_code;
|
||||
packet.seq_num = _seq_num;
|
||||
packet.data = data;
|
||||
packet.data_len = length;
|
||||
time_out = _time_out;
|
||||
}
|
||||
} Command;
|
||||
|
||||
class CommandChannelDelegate {
|
||||
public:
|
||||
virtual void OnCommand(uint8_t handle, const Command &command) = 0;
|
||||
virtual void OnHeartbeatStateUpdate(uint8_t handle, const HeartbeatResponse &state) = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* CommandChannel implements the sending/receiving commands with a specific device.
|
||||
*/
|
||||
class CommandChannel : public IOLoop::IOLoopDelegate {
|
||||
public:
|
||||
CommandChannel(apr_port_t port,
|
||||
uint8_t handle,
|
||||
const std::string &remote_ip,
|
||||
CommandChannelDelegate *cb,
|
||||
apr_pool_t *pool);
|
||||
virtual ~CommandChannel() { Uninit(); }
|
||||
|
||||
/** Uninitialize CommandChannel. */
|
||||
void Uninit();
|
||||
|
||||
/**
|
||||
* Bind a CommandChannel with a IOLoop.
|
||||
* @param loop the IOLoop to bind.
|
||||
* @return true on successfully.
|
||||
*/
|
||||
bool Bind(IOLoop *loop);
|
||||
|
||||
/**
|
||||
* Send a command asynchronously.
|
||||
* @param command the command to send.
|
||||
*/
|
||||
void SendAsync(const Command &command);
|
||||
|
||||
void OnData(apr_socket_t *, void *);
|
||||
void OnTimer(apr_time_t now);
|
||||
|
||||
static uint16_t GenerateSeq();
|
||||
|
||||
private:
|
||||
void Send(const Command &cmd);
|
||||
void HeartBeat(apr_time_t t);
|
||||
void SendInternal(const Command &command);
|
||||
Command DeepCopy(const Command &cmd);
|
||||
void OnHeartbeatAck(const CommPacket &packet);
|
||||
void DeviceDisconnect(uint8_t handle);
|
||||
|
||||
private:
|
||||
static const int kHeartbeatTimer = 800;
|
||||
uint8_t handle_;
|
||||
apr_port_t port_;
|
||||
apr_socket_t *sock_;
|
||||
apr_pool_t *mem_pool_;
|
||||
IOLoop *loop_;
|
||||
CommandChannelDelegate *callback_;
|
||||
std::map<uint16_t, std::pair<Command, apr_time_t> > commands_;
|
||||
boost::scoped_ptr<CommPort> comm_port_;
|
||||
apr_time_t heartbeat_time_;
|
||||
std::string remote_ip_;
|
||||
apr_time_t last_heartbeat_;
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
|
||||
#endif // LIVOX_COMMAND_CHANNEL_H_
|
||||
@@ -0,0 +1,179 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "command_handler.h"
|
||||
#include <boost/thread/lock_guard.hpp>
|
||||
#include <boost/thread/locks.hpp>
|
||||
#include "command_impl.h"
|
||||
#include "device_manager.h"
|
||||
#include "hub_command_handler.h"
|
||||
#include "lidar_command_handler.h"
|
||||
|
||||
using boost::lock_guard;
|
||||
using boost::mutex;
|
||||
using boost::shared_ptr;
|
||||
using std::list;
|
||||
using std::make_pair;
|
||||
using std::multimap;
|
||||
using std::pair;
|
||||
|
||||
namespace livox {
|
||||
|
||||
CommandHandler &command_handler() {
|
||||
static CommandHandler handler;
|
||||
return handler;
|
||||
}
|
||||
|
||||
uint16_t GetCommandTimeout(uint8_t command_set, uint8_t command_id) {
|
||||
switch (command_set) {
|
||||
case kCommandSetGeneral:
|
||||
assert(command_id < sizeof(GeneralCommandTimeout));
|
||||
return GeneralCommandTimeout[command_id];
|
||||
case kCommandSetLidar:
|
||||
assert(command_id < sizeof(LidarCommandTimeout));
|
||||
return LidarCommandTimeout[command_id];
|
||||
case kCommandSetHub:
|
||||
assert(command_id < sizeof(HubCommandTimeout));
|
||||
return HubCommandTimeout[command_id];
|
||||
}
|
||||
return KDefaultTimeOut;
|
||||
}
|
||||
|
||||
bool CommandHandler::AddDevice(const DeviceInfo &info) {
|
||||
if (impl_ == NULL) {
|
||||
DeviceMode mode = static_cast<DeviceMode>(device_manager().device_mode());
|
||||
if (mode == kDeviceModeHub) {
|
||||
impl_.reset(new HubCommandHandlerImpl(this, mem_pool_, loop_));
|
||||
} else if (mode == kDeviceModeLidar) {
|
||||
impl_.reset(new LidarCommandHandlerImpl(this, mem_pool_, loop_));
|
||||
}
|
||||
}
|
||||
if (impl_ == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return impl_->AddDevice(info);
|
||||
}
|
||||
|
||||
bool CommandHandler::Init(IOLoop *loop) {
|
||||
apr_status_t rv = apr_pool_create(&mem_pool_, NULL);
|
||||
if (rv != APR_SUCCESS) {
|
||||
return false;
|
||||
}
|
||||
|
||||
loop_ = loop;
|
||||
return true;
|
||||
}
|
||||
|
||||
void CommandHandler::Uninit() {
|
||||
loop_ = NULL;
|
||||
if (impl_) {
|
||||
impl_.reset(NULL);
|
||||
}
|
||||
|
||||
if (mem_pool_) {
|
||||
apr_pool_destroy(mem_pool_);
|
||||
mem_pool_ = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void CommandHandler::OnCommand(uint8_t handle, const Command &command) {
|
||||
if (command.packet.packet_type == kCommandTypeAck) {
|
||||
OnCommandAck(handle, command);
|
||||
} else if (command.packet.packet_type == kCommandTypeMsg) {
|
||||
OnCommandMsg(handle, command);
|
||||
}
|
||||
}
|
||||
|
||||
bool CommandHandler::SendCommand(uint8_t handle,
|
||||
uint8_t command_set,
|
||||
uint8_t command_id,
|
||||
uint8_t *data,
|
||||
uint16_t length,
|
||||
const shared_ptr<CommandCallback> &cb) {
|
||||
if (impl_ == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
Command cmd(handle,
|
||||
kCommandTypeCmd,
|
||||
command_set,
|
||||
command_id,
|
||||
CommandChannel::GenerateSeq(),
|
||||
data,
|
||||
length,
|
||||
GetCommandTimeout(command_set, command_id),
|
||||
cb);
|
||||
bool result = impl_->SendCommand(handle, cmd);
|
||||
return result;
|
||||
}
|
||||
|
||||
bool CommandHandler::RegisterPush(uint8_t handle,
|
||||
uint8_t command_set,
|
||||
uint8_t command_id,
|
||||
const shared_ptr<CommandCallback> &cb) {
|
||||
lock_guard<mutex> lock(mutex_);
|
||||
Command req(handle, kCommandTypeMsg, command_set, command_id, 0, NULL, 0, 0, cb);
|
||||
message_registers_.insert(make_pair(MakeKey(command_set, command_id), req));
|
||||
return true;
|
||||
}
|
||||
|
||||
void CommandHandler::OnCommandAck(uint8_t handle, const Command &command) {
|
||||
if (command.cb != NULL) {
|
||||
(*command.cb)(handle, command.packet.data);
|
||||
}
|
||||
}
|
||||
|
||||
void CommandHandler::OnCommandMsg(uint8_t handle, const Command &command) {
|
||||
list<Command> commands;
|
||||
{
|
||||
lock_guard<mutex> lock(mutex_);
|
||||
pair<multimap<uint16_t, Command>::iterator, multimap<uint16_t, Command>::iterator> ret;
|
||||
ret = message_registers_.equal_range(MakeKey(command.packet.cmd_set, command.packet.cmd_code));
|
||||
|
||||
for (multimap<uint16_t, Command>::iterator ite = ret.first; ite != ret.second; ++ite) {
|
||||
if (ite->second.handle == handle) {
|
||||
commands.push_back(ite->second);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (list<Command>::iterator ite = commands.begin(); ite != commands.end(); ++ite) {
|
||||
if (ite->cb) {
|
||||
(*ite->cb)(handle, command.packet.data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void CommandHandler::RemoveDevice(uint8_t handle) {
|
||||
if (impl_) {
|
||||
impl_->RemoveDevice(handle);
|
||||
};
|
||||
}
|
||||
|
||||
void CommandHandler::OnHeartbeatStateUpdate(uint8_t handle, const HeartbeatResponse &state) {
|
||||
device_manager().UpdateDeviceState(handle, state);
|
||||
}
|
||||
|
||||
} // namespace livox
|
||||
@@ -0,0 +1,109 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef LIVOX_COMMAND_HANDLER_H_
|
||||
#define LIVOX_COMMAND_HANDLER_H_
|
||||
|
||||
#include <boost/smart_ptr.hpp>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include "base/command_callback.h"
|
||||
#include "base/util.h"
|
||||
#include "command_channel.h"
|
||||
#include "device_discovery.h"
|
||||
#include "livox_sdk.h"
|
||||
|
||||
namespace livox {
|
||||
class CommandHandlerImpl;
|
||||
|
||||
class CommandHandler : public noncopyable {
|
||||
public:
|
||||
explicit CommandHandler() : mem_pool_(NULL), loop_(NULL) {}
|
||||
|
||||
bool Init(IOLoop *loop);
|
||||
void Uninit();
|
||||
|
||||
bool AddDevice(const DeviceInfo &info);
|
||||
void RemoveDevice(uint8_t handle);
|
||||
|
||||
bool SendCommand(uint8_t handle,
|
||||
uint8_t command_set,
|
||||
uint8_t command_id,
|
||||
uint8_t *data,
|
||||
uint16_t length,
|
||||
const boost::shared_ptr<CommandCallback> &cb);
|
||||
|
||||
bool RegisterPush(uint8_t handle,
|
||||
uint8_t command_set,
|
||||
uint8_t command_id,
|
||||
const boost::shared_ptr<CommandCallback> &cb);
|
||||
|
||||
void OnCommand(uint8_t handle, const Command &command);
|
||||
|
||||
void OnHeartbeatStateUpdate(uint8_t handle, const HeartbeatResponse &state);
|
||||
|
||||
private:
|
||||
inline uint16_t MakeKey(uint8_t command_set, uint8_t command_id) { return (command_set << 8) | command_id; }
|
||||
|
||||
void OnCommandAck(uint8_t handle, const Command &command);
|
||||
void OnCommandMsg(uint8_t handle, const Command &command);
|
||||
|
||||
private:
|
||||
std::multimap<uint16_t, Command> message_registers_;
|
||||
apr_pool_t *mem_pool_;
|
||||
boost::scoped_ptr<CommandHandlerImpl> impl_;
|
||||
boost::mutex mutex_;
|
||||
IOLoop *loop_;
|
||||
};
|
||||
|
||||
class CommandHandlerImpl : public CommandChannelDelegate {
|
||||
public:
|
||||
CommandHandlerImpl(CommandHandler *handler) : handler_(handler) {}
|
||||
virtual ~CommandHandlerImpl() { Uninit(); }
|
||||
virtual void Uninit(){};
|
||||
virtual bool AddDevice(const DeviceInfo &info) = 0;
|
||||
virtual bool RemoveDevice(uint8_t handle) = 0;
|
||||
|
||||
virtual bool SendCommand(uint8_t handle, const Command &command) = 0;
|
||||
|
||||
virtual void OnCommand(uint8_t handle, const Command &command) {
|
||||
if (handler_) {
|
||||
handler_->OnCommand(handle, command);
|
||||
}
|
||||
}
|
||||
|
||||
void OnHeartbeatStateUpdate(uint8_t handle, const HeartbeatResponse &state) {
|
||||
if (handler_) {
|
||||
handler_->OnHeartbeatStateUpdate(handle, state);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
CommandHandler *handler_;
|
||||
};
|
||||
|
||||
CommandHandler &command_handler();
|
||||
|
||||
} // namespace livox
|
||||
|
||||
#endif // LIVOX_COMMAND_HANDLER_H_
|
||||
@@ -0,0 +1,352 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include <livox_sdk.h>
|
||||
|
||||
#include "command_handler.h"
|
||||
#include "command_impl.h"
|
||||
#include "data_handler/data_handler.h"
|
||||
#include "device_manager.h"
|
||||
#include "livox_def.h"
|
||||
#include "livox_sdk.h"
|
||||
|
||||
using std::pair;
|
||||
using std::vector;
|
||||
using namespace livox;
|
||||
|
||||
void SetDeviceStateUpdateCallback(DeviceStateUpdateCallback cb) {
|
||||
device_manager().SetDeviceConnectedCallback(cb);
|
||||
}
|
||||
|
||||
void SetBroadcastCallback(DeviceBroadcastCallback cb) {
|
||||
device_manager().SetDeviceBroadcastCallback(cb);
|
||||
}
|
||||
|
||||
uint8_t AddHubToConnect(const char *broadcast_code, uint8_t *handle) {
|
||||
bool result = device_manager().AddListeningDevice(broadcast_code, kDeviceModeHub, *handle);
|
||||
if (result) {
|
||||
return kStatusSuccess;
|
||||
} else {
|
||||
return kStatusFailure;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t AddLidarToConnect(const char *broadcast_code, uint8_t *handle) {
|
||||
bool result = device_manager().AddListeningDevice(broadcast_code, kDeviceModeLidar, *handle);
|
||||
if (result) {
|
||||
return kStatusSuccess;
|
||||
} else {
|
||||
return kStatusFailure;
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t GetConnectedDevices(DeviceInfo *devices, uint8_t *size) {
|
||||
if (devices == NULL || size == NULL) {
|
||||
return kStatusFailure;
|
||||
}
|
||||
vector<DeviceInfo> device;
|
||||
device_manager().GetConnectedDevices(device);
|
||||
uint16_t final_size = (*size >= device.size()) ? device.size() : *size;
|
||||
for (int i = 0; i < final_size; i++) {
|
||||
devices[i] = device[i];
|
||||
}
|
||||
*size = final_size;
|
||||
return kStatusSuccess;
|
||||
}
|
||||
|
||||
void SetDataCallback(uint8_t handle, DataCallback cb) {
|
||||
data_handler().AddDataListener(handle, cb);
|
||||
}
|
||||
|
||||
uint8_t DeviceSampleControl(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) {
|
||||
uint8_t req = enable;
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetGeneral,
|
||||
kCommandIDGeneralControlSample,
|
||||
&req,
|
||||
sizeof(req),
|
||||
MakeCommandCallback<uint8_t>(cb, data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t LidarStartSampling(uint8_t handle, CommonCommandCallback cb, void *data) {
|
||||
if (device_manager().device_mode() != kDeviceModeLidar) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
return DeviceSampleControl(handle, true, cb, data);
|
||||
}
|
||||
|
||||
uint8_t LidarStopSampling(uint8_t handle, CommonCommandCallback cb, void *data) {
|
||||
if (device_manager().device_mode() != kDeviceModeLidar) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
return DeviceSampleControl(handle, false, cb, data);
|
||||
}
|
||||
|
||||
uint8_t HubStartSampling(CommonCommandCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
return DeviceSampleControl(kHubDefaultHandle, true, cb, client_data);
|
||||
}
|
||||
|
||||
uint8_t HubStopSampling(CommonCommandCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
return DeviceSampleControl(kHubDefaultHandle, false, cb, client_data);
|
||||
}
|
||||
|
||||
uint8_t HubGetLidarHandle(uint8_t slot, uint8_t id) {
|
||||
return (slot - 1) * 3 + id - 1;
|
||||
}
|
||||
|
||||
uint8_t QueryDeviceInformation(uint8_t handle, DeviceInformationCallback cb, void *data) {
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetGeneral,
|
||||
kCommandIDGeneralDeviceInfo,
|
||||
NULL,
|
||||
0,
|
||||
MakeCommandCallback<DeviceInformationResponse>(cb, data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t SetCartesianCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
|
||||
uint8_t req = 0;
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetGeneral,
|
||||
kCommandIDGeneralCoordinateSystem,
|
||||
&req,
|
||||
sizeof(req),
|
||||
MakeCommandCallback<uint8_t>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t SetSphericalCoordinate(uint8_t handle, CommonCommandCallback cb, void *client_data) {
|
||||
uint8_t req = 1;
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetGeneral,
|
||||
kCommandIDGeneralCoordinateSystem,
|
||||
&req,
|
||||
sizeof(req),
|
||||
MakeCommandCallback<uint8_t>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t SetErrorMessageCallback(uint8_t handle, ErrorMessageCallback cb) {
|
||||
bool result = command_handler().RegisterPush(
|
||||
handle, kCommandSetGeneral, kCommandIDGeneralPushAbnormalState, MakeMessageCallback<ErrorMessage>(cb));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t SetStaticDynamicIP(uint8_t handle,
|
||||
SetDeviceIPModeRequest *req,
|
||||
CommonCommandCallback cb,
|
||||
void *client_data) {
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetGeneral,
|
||||
kCommandIDGeneralConfigureStaticDynamicIP,
|
||||
(uint8_t *)req,
|
||||
sizeof(*req),
|
||||
MakeCommandCallback<uint8_t>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t GetDeviceIPInformation(uint8_t handle, GetDeviceIPInformationCallback cb, void *client_data) {
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetGeneral,
|
||||
kCommandIDGeneralGetDeviceIPInformation,
|
||||
NULL,
|
||||
0,
|
||||
MakeCommandCallback<GetDeviceIPModeResponse>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t LidarSetMode(uint8_t handle, LidarMode mode, CommonCommandCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeLidar) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
uint8_t req = static_cast<uint8_t>(mode);
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetLidar,
|
||||
kCommandIDLidarSetMode,
|
||||
&req,
|
||||
sizeof(req),
|
||||
MakeCommandCallback<uint8_t>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t LidarSetExtrinsicParameter(uint8_t handle,
|
||||
LidarSetExtrinsicParameterRequest *req,
|
||||
CommonCommandCallback cb,
|
||||
void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeLidar) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetLidar,
|
||||
kCommandIDLidarSetExtrinsicParameter,
|
||||
(uint8_t *)req,
|
||||
sizeof(*req),
|
||||
MakeCommandCallback<uint8_t>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t LidarGetExtrinsicParameter(uint8_t handle, LidarGetExtrinsicParameterCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeLidar) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetLidar,
|
||||
kCommandIDLidarGetExtrinsicParameter,
|
||||
NULL,
|
||||
0,
|
||||
MakeCommandCallback<LidarGetExtrinsicParameterResponse>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t LidarRainFogSuppress(uint8_t handle, bool enable, CommonCommandCallback cb, void *data) {
|
||||
uint8_t req = static_cast<uint8_t>(enable);
|
||||
bool result = command_handler().SendCommand(handle,
|
||||
kCommandSetLidar,
|
||||
kCommandIDLidarControlRainFogSuppression,
|
||||
&req,
|
||||
sizeof(req),
|
||||
MakeCommandCallback<uint8_t>(cb, data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t HubQueryLidarInformation(HubQueryLidarInformationCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(kHubDefaultHandle,
|
||||
kCommandSetHub,
|
||||
kCommandIDHubQueryLidarInformation,
|
||||
NULL,
|
||||
0,
|
||||
MakeCommandCallback<HubQueryLidarInformationResponse>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t HubSetMode(HubSetModeRequest *req, uint16_t length, HubSetModeCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(kHubDefaultHandle,
|
||||
kCommandSetHub,
|
||||
kCommandIDHubSetMode,
|
||||
(uint8_t *)req,
|
||||
length,
|
||||
MakeCommandCallback<HubSetModeResponse>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t HubControlSlotPower(HubControlSlotPowerRequest *req, CommonCommandCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(kHubDefaultHandle,
|
||||
kCommandSetHub,
|
||||
kCommandIDHubControlSlotPower,
|
||||
(uint8_t *)req,
|
||||
sizeof(*req),
|
||||
MakeCommandCallback<uint8_t>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t HubSetExtrinsicParameter(HubSetExtrinsicParameterRequest *req,
|
||||
uint16_t length,
|
||||
HubSetExtrinsicParameterCallback cb,
|
||||
void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(kHubDefaultHandle,
|
||||
kCommandSetHub,
|
||||
kCommandIDHubSetExtrinsicParameter,
|
||||
(uint8_t *)req,
|
||||
length,
|
||||
MakeCommandCallback<HubSetExtrinsicParameterResponse>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t HubGetExtrinsicParameter(HubGetExtrinsicParameterRequest *req,
|
||||
uint16_t length,
|
||||
HubGetExtrinsicParameterCallback cb,
|
||||
void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(kHubDefaultHandle,
|
||||
kCommandSetHub,
|
||||
kCommandIDHubGetExtrinsicParameter,
|
||||
(uint8_t *)req,
|
||||
length,
|
||||
MakeCommandCallback<HubGetExtrinsicParameterResponse>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t HubQueryLidarStatus(HubQueryLidarStatusCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(kHubDefaultHandle,
|
||||
kCommandSetHub,
|
||||
kCommandIDHubQueryLidarDeviceStatus,
|
||||
NULL,
|
||||
0,
|
||||
MakeCommandCallback<HubQueryLidarStatusResponse>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t HubExtrinsicParameterCalculation(bool enable, CommonCommandCallback cb, void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
uint8_t req = static_cast<uint8_t>(enable);
|
||||
bool result = command_handler().SendCommand(kHubDefaultHandle,
|
||||
kCommandSetHub,
|
||||
kCommandIDHubExtrinsicParameterCalculation,
|
||||
&req,
|
||||
sizeof(req),
|
||||
MakeCommandCallback<uint8_t>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
|
||||
uint8_t HubRainFogSuppress(HubRainFogSuppressRequest *req,
|
||||
uint16_t length,
|
||||
HubRainFogSuppressCallback cb,
|
||||
void *client_data) {
|
||||
if (device_manager().device_mode() != kDeviceModeHub) {
|
||||
return kStatusNotSupported;
|
||||
}
|
||||
bool result = command_handler().SendCommand(kHubDefaultHandle,
|
||||
kCommandSetHub,
|
||||
kCommandIDHubRainFogSuppression,
|
||||
(uint8_t *)req,
|
||||
length,
|
||||
MakeCommandCallback<HubRainFogSuppressResponse>(cb, client_data));
|
||||
return result ? kStatusSuccess : kStatusFailure;
|
||||
}
|
||||
@@ -0,0 +1,196 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef LIVOX_SDK_COMMAND_IMPL_H
|
||||
#define LIVOX_SDK_COMMAND_IMPL_H
|
||||
|
||||
namespace livox {
|
||||
|
||||
/** The maximum buffer size of command */
|
||||
static const uint32_t kMaxCommandBufferSize = 1536;
|
||||
static const uint16_t KDefaultTimeOut = 500;
|
||||
|
||||
/** Enum that represents the index of command set. */
|
||||
typedef enum {
|
||||
kCommandSetGeneral = 0, /**< general command set. */
|
||||
kCommandSetLidar, /**< LiDAR command set. */
|
||||
kCommandSetHub /**< hub command set. */
|
||||
} CommandSet;
|
||||
|
||||
/** Enum that represents the command id. */
|
||||
typedef enum {
|
||||
/** General command set, broadcast command. */
|
||||
kCommandIDGeneralBroadcast = 0,
|
||||
/**
|
||||
* General command set, handshake command. When received broadcast from a Livox LiDAR or Hub, a handshake command
|
||||
* should be sent back to the same port to negotiate connection with the device.
|
||||
*/
|
||||
kCommandIDGeneralHandshake = 1,
|
||||
/**
|
||||
* General command set, query the information of device.
|
||||
*/
|
||||
kCommandIDGeneralDeviceInfo = 2,
|
||||
/**
|
||||
* General command set, heartbeat command. after connected, a heartbeat command should be sent at least every second.
|
||||
*/
|
||||
kCommandIDGeneralHeartbeat = 3,
|
||||
/**
|
||||
* General command set, enable or disable the sampling.
|
||||
*
|
||||
*/
|
||||
kCommandIDGeneralControlSample = 4,
|
||||
/**
|
||||
* General command set, change the coordinate of point cloud data.
|
||||
*/
|
||||
kCommandIDGeneralCoordinateSystem = 5,
|
||||
/**
|
||||
* General command set, disconnect the device.
|
||||
*/
|
||||
kCommandIDGeneralDisconnect = 6,
|
||||
/**
|
||||
* General command set, a message command from a connected device to notify exceptions, in 10Hz.
|
||||
*/
|
||||
kCommandIDGeneralPushAbnormalState = 7,
|
||||
/**
|
||||
* General command set, set the IP of the a device.
|
||||
*/
|
||||
kCommandIDGeneralConfigureStaticDynamicIP = 8,
|
||||
/**
|
||||
* General command set, get the IP of the a device.
|
||||
*/
|
||||
kCommandIDGeneralGetDeviceIPInformation = 9,
|
||||
|
||||
/**
|
||||
* ******************************************************
|
||||
* Don't add command id after kCommandIDGeneralCommandCount.
|
||||
*/
|
||||
kCommandIDGeneralCommandCount
|
||||
} GeneralCommandID;
|
||||
|
||||
static const uint16_t GeneralCommandTimeout[kCommandIDGeneralCommandCount] = {KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut};
|
||||
|
||||
/** Enum that represents the command id. */
|
||||
typedef enum {
|
||||
/**
|
||||
* Lidar command set, set the working mode and sub working mode of a LiDAR.
|
||||
*/
|
||||
kCommandIDLidarSetMode = 0,
|
||||
/**
|
||||
* Lidar command set, set the parameters of a LiDAR.
|
||||
*/
|
||||
kCommandIDLidarSetExtrinsicParameter = 1,
|
||||
/**
|
||||
* Lidar command set, get the parameters of a LiDAR.
|
||||
*/
|
||||
kCommandIDLidarGetExtrinsicParameter = 2,
|
||||
/**
|
||||
* Lidar command set, enable or disable the rain/fog suppression of a LiDAR.
|
||||
*/
|
||||
kCommandIDLidarControlRainFogSuppression = 3,
|
||||
|
||||
/**
|
||||
* ******************************************************
|
||||
* Don't add command id after kCommandIDLidarCommandCount.
|
||||
*/
|
||||
kCommandIDLidarCommandCount
|
||||
} LidarCommandID;
|
||||
|
||||
static const uint16_t LidarCommandTimeout[kCommandIDLidarCommandCount] = {KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut};
|
||||
|
||||
/** Enum that represents the command id. */
|
||||
typedef enum {
|
||||
/**
|
||||
* Hub command set, get the information of connected Livox LiDAR.
|
||||
*/
|
||||
kCommandIDHubQueryLidarInformation = 0,
|
||||
/**
|
||||
* Hub command set, set the working mode of connected Livox LiDAR.
|
||||
*/
|
||||
kCommandIDHubSetMode = 1,
|
||||
/**
|
||||
* Hub command set, enable or disable the power supply of the hub slot.
|
||||
*/
|
||||
kCommandIDHubControlSlotPower = 2,
|
||||
/**
|
||||
* Hub command set, set the parameters of connected Livox LiDAR.
|
||||
*/
|
||||
kCommandIDHubSetExtrinsicParameter = 3,
|
||||
/**
|
||||
* Hub command set, get the parameters of connected Livox LiDAR.
|
||||
*/
|
||||
kCommandIDHubGetExtrinsicParameter = 4,
|
||||
/**
|
||||
* Hub command set, get the working state of the connected Livox LiDAR.
|
||||
*/
|
||||
kCommandIDHubQueryLidarDeviceStatus = 5,
|
||||
/**
|
||||
* Hub command set, enable or disable calculating the parameters.
|
||||
*/
|
||||
kCommandIDHubExtrinsicParameterCalculation = 6,
|
||||
/**
|
||||
* Hub command set, open or close the rain and fog mode of the connected Livox LiDAR.
|
||||
*/
|
||||
kCommandIDHubRainFogSuppression = 7,
|
||||
|
||||
/**
|
||||
* ******************************************************
|
||||
* Don't add command id after kCommandIDHubCommandCount.
|
||||
*/
|
||||
kCommandIDHubCommandCount
|
||||
} HubCommandID;
|
||||
|
||||
static const uint16_t HubCommandTimeout[kCommandIDHubCommandCount] = {KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut,
|
||||
KDefaultTimeOut};
|
||||
|
||||
/**
|
||||
* Enum that represents the command type.
|
||||
*/
|
||||
typedef enum {
|
||||
/** command type, which requires response from the receiver. */
|
||||
kCommandTypeCmd = 0,
|
||||
/** acknowledge type, which is the response of command type. */
|
||||
kCommandTypeAck = 1,
|
||||
/** message type, which is sent at a specified frequency. */
|
||||
kCommandTypeMsg = 2
|
||||
} CommandType;
|
||||
} // namespace livox
|
||||
#endif // LIVOX_SDK_COMMAND_IMPL_H
|
||||
@@ -0,0 +1,65 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "hub_command_handler.h"
|
||||
#include "base/network_util.h"
|
||||
|
||||
namespace livox {
|
||||
|
||||
void HubCommandHandlerImpl::Uninit() {
|
||||
if (channel_) {
|
||||
channel_.reset(NULL);
|
||||
}
|
||||
is_valid_ = false;
|
||||
}
|
||||
|
||||
bool HubCommandHandlerImpl::AddDevice(const DeviceInfo &info) {
|
||||
if (is_valid_) {
|
||||
return false;
|
||||
}
|
||||
is_valid_ = true;
|
||||
hub_info_ = info;
|
||||
|
||||
channel_.reset(new CommandChannel(info.cmd_port, info.handle, info.ip, this, mem_pool_));
|
||||
channel_->Bind(loop_);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HubCommandHandlerImpl::SendCommand(uint8_t, const Command &command) {
|
||||
if (channel_ == NULL) {
|
||||
return false;
|
||||
}
|
||||
channel_->SendAsync(command);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool HubCommandHandlerImpl::RemoveDevice(uint8_t) {
|
||||
is_valid_ = false;
|
||||
if (channel_) {
|
||||
channel_.reset(NULL);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
} // namespace livox
|
||||
@@ -0,0 +1,49 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef LIVOX_HUB_COMMAND_HANDLER_H_
|
||||
#define LIVOX_HUB_COMMAND_HANDLER_H_
|
||||
|
||||
#include <boost/scoped_ptr.hpp>
|
||||
#include "command_handler.h"
|
||||
|
||||
namespace livox {
|
||||
class HubCommandHandlerImpl : public CommandHandlerImpl {
|
||||
public:
|
||||
HubCommandHandlerImpl(CommandHandler *handler, apr_pool_t *pool, IOLoop *loop)
|
||||
: CommandHandlerImpl(handler), mem_pool_(pool), loop_(loop), is_valid_(false) {}
|
||||
void Uninit();
|
||||
bool AddDevice(const DeviceInfo &info);
|
||||
bool RemoveDevice(uint8_t handle);
|
||||
bool SendCommand(uint8_t handle, const Command &command);
|
||||
|
||||
private:
|
||||
apr_pool_t *mem_pool_;
|
||||
IOLoop *loop_;
|
||||
bool is_valid_;
|
||||
DeviceInfo hub_info_;
|
||||
boost::scoped_ptr<CommandChannel> channel_;
|
||||
};
|
||||
} // namespace livox
|
||||
#endif // LIVOX_HUB_COMMAND_HANDLER_H_
|
||||
@@ -0,0 +1,74 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#include "lidar_command_handler.h"
|
||||
|
||||
using std::list;
|
||||
|
||||
namespace livox {
|
||||
|
||||
void LidarCommandHandlerImpl::Uninit() {
|
||||
devices_.clear();
|
||||
}
|
||||
|
||||
bool LidarCommandHandlerImpl::AddDevice(const DeviceInfo &info) {
|
||||
boost::shared_ptr<CommandChannel> channel =
|
||||
boost::make_shared<CommandChannel>(info.cmd_port, info.handle, info.ip, this, mem_pool_);
|
||||
channel->Bind(loop_);
|
||||
|
||||
DeviceItem item = {channel, info};
|
||||
devices_.push_back(item);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool LidarCommandHandlerImpl::SendCommand(uint8_t handle, const Command &command) {
|
||||
CommandChannel *channel = NULL;
|
||||
for (list<DeviceItem>::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) {
|
||||
if (ite->info.handle == handle) {
|
||||
channel = ite->channel.get();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (channel) {
|
||||
channel->SendAsync(command);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool LidarCommandHandlerImpl::RemoveDevice(uint8_t handle) {
|
||||
bool found = false;
|
||||
for (list<DeviceItem>::iterator ite = devices_.begin(); ite != devices_.end(); ++ite) {
|
||||
if (ite->info.handle == handle) {
|
||||
found = true;
|
||||
devices_.erase(ite);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return found;
|
||||
}
|
||||
|
||||
} // namespace livox
|
||||
@@ -0,0 +1,55 @@
|
||||
//
|
||||
// The MIT License (MIT)
|
||||
//
|
||||
// Copyright (c) 2019 Livox. All rights reserved.
|
||||
//
|
||||
// Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
// of this software and associated documentation files (the "Software"), to deal
|
||||
// in the Software without restriction, including without limitation the rights
|
||||
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
// copies of the Software, and to permit persons to whom the Software is
|
||||
// furnished to do so, subject to the following conditions:
|
||||
//
|
||||
// The above copyright notice and this permission notice shall be included in
|
||||
// all copies or substantial portions of the Software.
|
||||
//
|
||||
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
// SOFTWARE.
|
||||
//
|
||||
|
||||
#ifndef LIVOX_LIDAR_COMMAND_HANDLER_H_
|
||||
#define LIVOX_LIDAR_COMMAND_HANDLER_H_
|
||||
#include <boost/smart_ptr.hpp>
|
||||
#include "command_handler.h"
|
||||
|
||||
namespace livox {
|
||||
|
||||
class LidarCommandHandlerImpl : public CommandHandlerImpl {
|
||||
public:
|
||||
LidarCommandHandlerImpl(CommandHandler *handler, apr_pool_t *pool, IOLoop *loop)
|
||||
: CommandHandlerImpl(handler), mem_pool_(pool), loop_(loop) {}
|
||||
|
||||
void Uninit();
|
||||
|
||||
bool AddDevice(const DeviceInfo &info);
|
||||
bool RemoveDevice(uint8_t handle);
|
||||
bool SendCommand(uint8_t handle, const Command &command);
|
||||
|
||||
private:
|
||||
typedef struct {
|
||||
boost::shared_ptr<CommandChannel> channel;
|
||||
DeviceInfo info;
|
||||
} DeviceItem;
|
||||
apr_pool_t *mem_pool_;
|
||||
std::list<DeviceItem> devices_;
|
||||
IOLoop *loop_;
|
||||
};
|
||||
|
||||
} // namespace livox
|
||||
|
||||
#endif // LIVOX_LIDAR_COMMAND_HANDLER_H_
|
||||
Reference in New Issue
Block a user