Robotics StackExchange | Archived questions

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

Comments

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