mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 07:13:34 +08:00
Add Zenoh pico support
This commit is contained in:
Peter van der Perk
committed by
Daniel Agar
Daniel Agar
parent
5137ca1ccc
commit
019d232911
@@ -0,0 +1,107 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file uorb_subscriber.hpp
|
||||
*
|
||||
* Defines generic, templatized uORB over Zenoh / ROS2
|
||||
*
|
||||
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "zenoh_subscriber.hpp"
|
||||
#include <uORB/topics/input_rc.h>
|
||||
#include <uORB/PublicationMulti.hpp>
|
||||
#include <uORB/topics/actuator_outputs.h>
|
||||
|
||||
class uORB_Zenoh_Subscriber : public Zenoh_Subscriber
|
||||
{
|
||||
public:
|
||||
uORB_Zenoh_Subscriber(const orb_metadata *meta, const uint32_t *ops) :
|
||||
Zenoh_Subscriber(true),
|
||||
_uorb_meta{meta},
|
||||
_cdr_ops(ops)
|
||||
{
|
||||
int instance = 0;
|
||||
_uorb_pub_handle = orb_advertise_multi_queue(_uorb_meta, nullptr, &instance, 1); //FIXME template magic qsize
|
||||
};
|
||||
|
||||
~uORB_Zenoh_Subscriber() override = default;
|
||||
|
||||
// Update the uORB Subscription and broadcast a Zenoh ROS2 message
|
||||
void data_handler(const z_sample_t *sample)
|
||||
{
|
||||
char data[_uorb_meta->o_size];
|
||||
|
||||
dds_istream_t is = {.m_buffer = (unsigned char *)(sample->payload.start), .m_size = static_cast<int>(sample->payload.len),
|
||||
.m_index = 4, .m_xcdr_version = DDSI_RTPS_CDR_ENC_VERSION_2
|
||||
};
|
||||
dds_stream_read(&is, data, &dds_allocator, _cdr_ops);
|
||||
|
||||
// As long as we don't have timesynchronization between Zenoh nodes
|
||||
// we've to manually set the timestamp
|
||||
fix_timestamp(data);
|
||||
|
||||
// ORB_ID::input_rc needs additional timestamp fixup
|
||||
if (static_cast<ORB_ID>(_uorb_meta->o_id) == ORB_ID::input_rc) {
|
||||
memcpy(&data[8], data, sizeof(hrt_abstime));
|
||||
}
|
||||
|
||||
orb_publish(_uorb_meta, _uorb_pub_handle, &data);
|
||||
};
|
||||
|
||||
void fix_timestamp(char *data)
|
||||
{
|
||||
hrt_abstime now = hrt_absolute_time();
|
||||
memcpy(data, &now, sizeof(hrt_abstime));
|
||||
}
|
||||
|
||||
void print()
|
||||
{
|
||||
Zenoh_Subscriber::print("uORB", _uorb_meta->o_name);
|
||||
}
|
||||
|
||||
protected:
|
||||
// Default payload-size function -- can specialize in derived class
|
||||
size_t get_payload_size()
|
||||
{
|
||||
return _uorb_meta->o_size;
|
||||
}
|
||||
|
||||
private:
|
||||
const orb_metadata *_uorb_meta;
|
||||
orb_advert_t _uorb_pub_handle;
|
||||
const uint32_t *_cdr_ops;
|
||||
};
|
||||
@@ -0,0 +1,111 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file zenoh_subscriber.cpp
|
||||
*
|
||||
* Zenoh subscriber
|
||||
*
|
||||
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||
*/
|
||||
|
||||
#include "zenoh_subscriber.hpp"
|
||||
|
||||
static void data_handler_cb(const z_sample_t *sample, void *arg)
|
||||
{
|
||||
static_cast<Zenoh_Subscriber *>(arg)->data_handler(sample);
|
||||
}
|
||||
|
||||
void Zenoh_Subscriber::data_handler(const z_sample_t *sample)
|
||||
{
|
||||
z_owned_str_t keystr = z_keyexpr_to_string(sample->keyexpr);
|
||||
printf(">> [Subscriber] Received ('%s' size '%d')\n", z_str_loan(&keystr), (int)sample->payload.len);
|
||||
z_str_drop(z_str_move(&keystr));
|
||||
}
|
||||
|
||||
|
||||
Zenoh_Subscriber::Zenoh_Subscriber(bool rostopic)
|
||||
{
|
||||
this->_rostopic = rostopic;
|
||||
this->_topic[0] = 0x0;
|
||||
}
|
||||
|
||||
Zenoh_Subscriber::~Zenoh_Subscriber()
|
||||
{
|
||||
undeclare_subscriber();
|
||||
}
|
||||
|
||||
int Zenoh_Subscriber::undeclare_subscriber()
|
||||
{
|
||||
z_undeclare_subscriber(z_subscriber_move(&_sub));
|
||||
return 0;
|
||||
}
|
||||
|
||||
int Zenoh_Subscriber::declare_subscriber(z_session_t s, const char *keyexpr)
|
||||
{
|
||||
z_owned_closure_sample_t callback = z_closure_sample(data_handler_cb, NULL, this);
|
||||
|
||||
if (_rostopic) {
|
||||
strncpy(this->_topic, (char *)_rt_prefix, _rt_prefix_offset);
|
||||
|
||||
if (keyexpr[0] == '/') {
|
||||
strncpy(this->_topic + _rt_prefix_offset, keyexpr + 1, sizeof(this->_topic) - _rt_prefix_offset);
|
||||
|
||||
} else {
|
||||
strncpy(this->_topic + _rt_prefix_offset, keyexpr, sizeof(this->_topic) - _rt_prefix_offset);
|
||||
}
|
||||
|
||||
} else {
|
||||
strncpy(this->_topic, (char *)keyexpr, sizeof(this->_topic));
|
||||
}
|
||||
|
||||
_sub = z_declare_subscriber(s, z_keyexpr(this->_topic), z_closure_sample_move(&callback), NULL);
|
||||
|
||||
|
||||
if (!z_subscriber_check(&_sub)) {
|
||||
printf("Unable to declare subscriber for key expression!\n %s\n", keyexpr);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void Zenoh_Subscriber::print()
|
||||
{
|
||||
printf("Topic: %s\n", this->_topic);
|
||||
}
|
||||
|
||||
void Zenoh_Subscriber::print(const char *type_string, const char *topic_string)
|
||||
{
|
||||
printf("Topic: %s -> %s %s \n", this->_topic, type_string, topic_string);
|
||||
}
|
||||
@@ -0,0 +1,80 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2023 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file zenoh_subscriber.hpp
|
||||
*
|
||||
* Defines basic functionality of Zenoh subscriber class
|
||||
*
|
||||
* @author Peter van der Perk <peter.vanderperk@nxp.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
|
||||
#include <lib/parameters/param.h>
|
||||
#include <containers/List.hpp>
|
||||
#include <zenoh-pico.h>
|
||||
|
||||
// CycloneDDS CDR Deserializer
|
||||
#include <dds/cdr/dds_cdrstream.h>
|
||||
#include <dds_serializer.h>
|
||||
|
||||
class Zenoh_Subscriber : public ListNode<Zenoh_Subscriber *>
|
||||
{
|
||||
public:
|
||||
Zenoh_Subscriber(bool rostopic = true);
|
||||
virtual ~Zenoh_Subscriber();
|
||||
|
||||
virtual int declare_subscriber(z_session_t s, const char *keyexpr);
|
||||
|
||||
virtual int undeclare_subscriber();
|
||||
|
||||
virtual void data_handler(const z_sample_t *sample);
|
||||
|
||||
virtual void print();
|
||||
|
||||
protected:
|
||||
virtual void print(const char *type_string, const char *topic_string);
|
||||
|
||||
z_owned_subscriber_t _sub;
|
||||
char _topic[60]; // The Topic name is somewhere is the Zenoh stack as well but no good api to fetch it.
|
||||
|
||||
|
||||
// Indicates ROS2 Topic namespace
|
||||
bool _rostopic;
|
||||
const char *_rt_prefix = "rt/";
|
||||
const size_t _rt_prefix_offset = 3; // "rt/" are 3 chars
|
||||
};
|
||||
Reference in New Issue
Block a user