diff --git a/include/tron_adapter.h b/include/tron_adapter.h index b353d738323be99c07dbcfe0301e3dc47bd7ec99..708f89fa354599620ec2d538bfa9238ed8c2d7fe 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 44920de90ef26c7fcfc557a8cc16c9f2d34dc6f6..b8ff03005b606baf23827fbe6f345920b3a41aff 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 7cd054d5ff14ee1dff787bf26c0ab66a87941991..3572993abd379298177d2f2d38b262c839f0b882 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 995eac4554b95ef5a4d60a41013ed1ef4ffee198..4e1b944f701f58c759f000d405ca433d18877fe6 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"; }