Skip to content

Instantly share code, notes, and snippets.

View youtalk's full-sized avatar

Yutaka Kondo youtalk

View GitHub Profile
@youtalk
youtalk / node.cpp
Last active December 24, 2017 13:00
Node::create_wall_timer
template<typename DurationT, typename CallbackT>
typename rclcpp::WallTimer<CallbackT>::SharedPtr
Node::create_wall_timer(
std::chrono::duration<int64_t, DurationT> period,
CallbackT callback,
rclcpp::callback_group::CallbackGroup::SharedPtr group)
{
auto timer = rclcpp::WallTimer<CallbackT>::make_shared(
std::chrono::duration_cast<std::chrono::nanoseconds>(period),
std::move(callback));
@youtalk
youtalk / node.cpp
Last active December 24, 2017 12:26
rclcpp::node_interfaces::Node***Interface
Node::Node(
const std::string & node_name,
const std::string & namespace_,
rclcpp::Context::SharedPtr context,
bool use_intra_process_comms)
: node_base_(new rclcpp::node_interfaces::NodeBase(node_name, namespace_, context)),
node_graph_(new rclcpp::node_interfaces::NodeGraph(node_base_.get())),
node_logging_(new rclcpp::node_interfaces::NodeLogging(node_base_.get())),
node_timers_(new rclcpp::node_interfaces::NodeTimers(node_base_.get())),
node_topics_(new rclcpp::node_interfaces::NodeTopics(node_base_.get())),
@youtalk
youtalk / serialize_std_msgs__string.c
Created July 2, 2017 22:34
FreeRTPS String Serializer
uint32_t serialize_std_msgs__string(void *_msg, uint8_t *_buf, uint32_t _buf_size)
{
struct std_msgs__string *_s = (struct std_msgs__string *)_msg;
uint8_t *_p = _buf;
if ((uintptr_t)_p & 3)
_p += 4 - ((uintptr_t)_p & 3);
uint32_t _data_len = (uint32_t)strlen(_s->data) + 1;
*((uint32_t *)_p) = _data_len;
_p += 4;
memcpy(_p, _s->data, _data_len);
@youtalk
youtalk / frudp_publish_user_msg.c
Last active July 2, 2017 22:27
FreeRTPS Publisher
bool frudp_publish_user_msg(frudp_pub_t *pub,
const uint8_t *payload, const uint32_t payload_len)
{
if (pub->reliable)
{
// 高信頼通信は未対応
FREERTPS_ERROR("user reliable publishing not quite done yet.\n");
return false;
}
@youtalk
youtalk / listener.c
Last active June 27, 2017 22:24
freertps apps
#include <stdio.h>
#include "freertps/freertps.h"
// コールバック関数
void chatter_cb(const void *msg)
{
// str_lenの型は32ビットなのに、forループの中では8ビット受け渡しなのが解せない
uint32_t str_len = *((uint32_t *)msg);
char buf[128] = {0};
for (int i = 0; i < str_len && i < sizeof(buf)-1; i++)
@youtalk
youtalk / test_parameters_async.cpp
Last active June 19, 2017 22:51
ROS 2 Paramter Async Test
auto parameters = parameters_client->get_parameters({"parameter_number", "parameter_string", "parameter_boolean"});
if (rclcpp::spin_until_future_complete(node, parameters) != rclcpp::executor::FutureReturnCode::SUCCESS)
{
std::cerr << "Failed to set parameter: " << result.reason << std::endl;
return -1;
}
for (auto & parameter : parameters.get()) {
std::cout << "Parameter name: " << parameter.get_name() << std::endl;
std::cout << "Parameter value (" << parameter.get_type_name() << "): " <<
parameter.value_to_string() << std::endl;
@youtalk
youtalk / test_parameters.cpp
Last active June 20, 2017 02:31
ROS 2 Parameter Test
int main(int argc, char ** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("get_parameters");
auto parameter_service = std::make_shared<rclcpp::parameter_service::ParameterService>(node);
auto parameters_client = std::make_shared<rclcpp::parameter_client::SyncParametersClient>(node);
// パラメータの設定
auto set_parameters_results = parameters_client->set_parameters({
rclcpp::parameter::ParameterVariant("parameter_number", 1),
rclcpp::parameter::ParameterVariant("parameter_string", "hello"),
@youtalk
youtalk / constructor.cpp
Last active June 16, 2017 06:19
rclcpp::parameter::ParameterVariant
explicit ParameterVariant(const std::string & name, const bool bool_value);
explicit ParameterVariant(const std::string & name, const int int_value);
explicit ParameterVariant(const std::string & name, const int64_t int_value);
explicit ParameterVariant(const std::string & name, const float double_value);
explicit ParameterVariant(const std::string & name, const double double_value);
@youtalk
youtalk / add_callback.cpp
Last active June 16, 2017 06:17
rclcpp::callback_group::CallbackGroup
void
add_subscription(const rclcpp::subscription::SubscriptionBase::SharedPtr subscription_ptr);
void
add_timer(const rclcpp::timer::TimerBase::SharedPtr timer_ptr);
void
add_service(const rclcpp::service::ServiceBase::SharedPtr service_ptr);
void
@youtalk
youtalk / CallbackGroupType.cpp
Last active June 15, 2017 21:42
rclcpp::callback_group::CallbackGroupType
enum class CallbackGroupType
{
MutuallyExclusive,
Reentrant
};