From 4bcc17cdeebb07dcd89f65e2774f008f87ce8588 Mon Sep 17 00:00:00 2001
From: CS <christoph.schroeter1@mailbox.tu-dresden.de>
Date: Sat, 14 Aug 2021 23:59:08 +0200
Subject: [PATCH]
---
include/tron_adapter.h | 7 +++----
src/garage_adapter.cpp | 22 ++++++++++++++++++++--
src/garage_server.cpp | 16 ++++++++++++++++
src/tron_adapter.cpp | 2 +-
4 files changed, 40 insertions(+), 7 deletions(-)
diff --git a/include/tron_adapter.h b/include/tron_adapter.h
index b353d73..708f89f 100644
--- a/include/tron_adapter.h
+++ b/include/tron_adapter.h
@@ -104,7 +104,7 @@ struct Mapping {
Channel channel = Channel("", 0, true); // TRON channel
// Callback used if TRON sends some message
// output callbacks must be specified when subscribing to topics
- void(*input_callback)(Mapping& map, int32_t*) = nullptr;
+ boost::function<void(Mapping& map, int32_t*)> input_callback = 0;
// note: since ROS messages can be complex, using byte positions might be cumbersome to use
// it is easy to implement custom callbacks using ROS message types
@@ -258,7 +258,7 @@ struct TRON_Adapter {
pub.publish(ptr);
return;
}
- throw "did not find publisher for topic";
+ throw "did not find publisher";
}
/**
@@ -292,8 +292,7 @@ struct TRON_Adapter {
}
publish_to_topic<T>(map.topic, shared_ptr);
}
-
-
+
/**
* callback reports to TRON like defined in byte-mappings
* note: this callback reports to all channels associated with calling topic
diff --git a/src/garage_adapter.cpp b/src/garage_adapter.cpp
index 44920de..b8ff030 100644
--- a/src/garage_adapter.cpp
+++ b/src/garage_adapter.cpp
@@ -1,6 +1,7 @@
#include <ros/ros.h>
#include <actionlib_example/TriggerAction.h>
#include "tron_adapter.h"
+#include <std_msgs/Int32.h>
TRON_Adapter adapter;
@@ -70,11 +71,28 @@ int main(int argc, char**argv){
const std::string IP = "127.0.0.1";
const uint16_t PORT = 8080;
adapter = TRON_Adapter(IP, PORT);
-
+
configuration_phase(nh);
- adapter.request_start();
+ // example use of template callbacks
+ /*
+ Mapping map = adapter.createMapping("/test_in", "oeffnen", true);
+ adapter.add_var_to_mapping(map, "akt_position");
+ map.input_callback = boost::bind(&TRON_Adapter::mapping_callback_to_topic<std_msgs::Int32>, &adapter, _1, _2);
+ adapter.input_publishers.push_back(nh.advertise<std_msgs::Int32>("/test_in", 10));
+ adapter.mappings.push_back(map);
+ map = adapter.createMapping("/test_out", "position", false);
+ adapter.add_var_to_mapping(map, "akt_position");
+ adapter.output_subscribers.push_back(nh.subscribe<std_msgs::Int32>("/test_out", 10,
+ boost::function<void(const ros::MessageEvent<std_msgs::Int32>&)>(
+ boost::bind(&TRON_Adapter::mappings_callback_to_TRON<std_msgs::Int32>, &adapter, _1))));
+ adapter.mappings.push_back(map);
+ adapter.set_time_unit_and_timeout(1000000L, 300);
+ */
+
+ adapter.request_start();
+
// testing phase loop
ros::Rate test_phase_freq(60);
while (ros::ok()) {
diff --git a/src/garage_server.cpp b/src/garage_server.cpp
index 7cd054d..3572993 100644
--- a/src/garage_server.cpp
+++ b/src/garage_server.cpp
@@ -2,6 +2,7 @@
#include <actionlib_example/TriggerAction.h>
#include <actionlib/server/simple_action_server.h>
#include <stdlib.h>
+#include <std_msgs/Int32.h>
class Garage {
private:
@@ -61,14 +62,29 @@ class Garage {
}
};
+
+/*
+ros::Publisher pub;
+void test_in_cb(ros::MessageEvent<std_msgs::Int32> event) {
+ std_msgs::Int32 i;
+ i.data = 100;
+ pub.publish(i);
+}
+*/
+
int main(int argc, char** argv){
ros::init(argc, argv, "gar_serv");
+
ros::NodeHandle nh;
ROS_INFO("server here");
// not working on stack for some reason,
// might be too big, no stackoverflow thrown though
// Garage gar();
Garage* gar = new Garage();
+ /*
+ pub = nh.advertise<std_msgs::Int32>("test_out", 10);
+ ros::Subscriber sub = nh.subscribe<std_msgs::Int32>("test_in", 10, test_in_cb);
+ */
ros::spin();
delete gar;
return 0;
diff --git a/src/tron_adapter.cpp b/src/tron_adapter.cpp
index 995eac4..4e1b944 100644
--- a/src/tron_adapter.cpp
+++ b/src/tron_adapter.cpp
@@ -308,7 +308,7 @@ void TRON_Adapter::process_TRONs_msgs(){
for (Mapping& map : mappings)
if (map.channel.name == chan->name && map.channel.is_input) {
- if (map.input_callback != nullptr)
+ if (!map.input_callback.empty())
map.input_callback(map, vals);
else throw "no callback declared";
}
--
GitLab