ROS2 how to assign callback to publisher node without create_wall_timer.
I am using ROS2 Foxy and am trying to create my own package for the VectorNav vn300 IMU.
The VectorNav library provides a way to call a function every time there is a packet received from the device (registerAsyncPacketReceivedHandler). Here is the node initialization:
VectorNavPublisher(std::string vec_port, int vec_baud, int rate)
: Node("vectornav")
{
publisher = this->create_publisher<std_msgs::msg::String>("vn_msg", 10);
// import and setup vectornav here then do the publishing in the Async handler
VnSensor vs;
connectVs(vs, vec_port, vec_baud);
std::string mn = vs.readModelNumber();
RCLCPP_INFO(this->get_logger(), "VectorNav connected. Model Number: %s", mn);
AsciiAsync asciiAsync = (AsciiAsync) 0;
vs.writeAsyncDataOutputType(asciiAsync); // Turns on Binary Message Type
SynchronizationControlRegister scr( // synchronizes vecnav off of DAQ clock and triggers at desired rate
SYNCINMODE_COUNT, // SYNCINMODE_ASYNC, TODO: test sync_in on another vn300, this one is unresponsive
SYNCINEDGE_RISING,
0, //(int)(daq_rate/vec_rate-1), // Setting skip factor so that the trigger happens at the desired rate.
SYNCOUTMODE_IMUREADY,
SYNCOUTPOLARITY_POSITIVE,
0,
1250000);
vs.writeSynchronizationControl(scr);
BinaryOutputRegister bor(
ASYNCMODE_PORT1,
400/rate,
COMMONGROUP_TIMEGPS | COMMONGROUP_YAWPITCHROLL | COMMONGROUP_ANGULARRATE | COMMONGROUP_POSITION | COMMONGROUP_VELOCITY | COMMO
TIMEGROUP_NONE,
IMUGROUP_TEMP | IMUGROUP_PRES,
GPSGROUP_NONE,
ATTITUDEGROUP_YPRU,
INSGROUP_POSU | INSGROUP_VELU,
GPSGROUP_NONE);
vs.writeBinaryOutput1(bor);
// setup vs configuration (TODO: get some of this from YAML)
vs.writeAsyncDataOutputFrequency(1);
UserData ud;
ud.vn = this; // the Async handler needs to have the current object passed in as a pointer, its not ideal but it is less hacky t
vs.registerAsyncPacketReceivedHandler(&ud, vecnavBinaryEventHandle);
for(;;){} // without for loop it segfaults on ros.spin()
}
I realize that this does not tell ROS that the callback "vecnavBinaryEventHandle" is associated to my node in any way. How do I tell ROS (and ros.spin()) that vecnavBinaryEventHandle is a callback functions of my node?
All examples I can find of basic publishers use createwalltimer to tell the node where the callback is (got this from: https://answers.ros.org/question/307200/in-ros2-is-a-wall-timer-connected-to-a-node-in-a-way-meaningfull-for-a-user/), but I do not want this to execute on a timer, I want it to execute when there is a message from the vectornav.
I have this code up on GitHub at: https://github.com/jashley2017/vectornav_ros2 for better context. It can run after a basic colcon and 'ros run vectornav vectornav'.
Thanks in advance!
Asked by jashley2017 on 2020-07-21 11:23:02 UTC
Answers
https://gist.github.com/pedro1713/68bcd6bb693d48b646f70e6ddf77595f maybe this helps. Different device but similar situation. I register callbacks for new data from the GPS unit.
Asked by pedro1713 on 2020-07-21 12:52:46 UTC
Comments
I do not have anything to "spin" on like you have in your gist in the run() method. Could I just put something like this:
void run(int rate){
rclcpp::WallRate loop_rate(rate);
while(rclcpp::ok()){
loop_rate.sleep();
}
}
Asked by jashley2017 on 2020-07-22 10:01:15 UTC
Comments