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/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"; }