ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

ROS2 how to assign callback to publisher node without create_wall_timer.

asked 2020-07-21 11:23:02 -0500

jashley2017 gravatar image

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 create_wall_timer to tell the node where the callback is (got this from: https://answers.ros.org/question/3072...), 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/vector... for better context. It can run after a basic colcon and 'ros run vectornav vectornav'.

Thanks in advance!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-07-21 12:52:46 -0500

pedro1713 gravatar image

https://gist.github.com/pedro1713/68b... maybe this helps. Different device but similar situation. I register callbacks for new data from the GPS unit.

edit flag offensive delete link more

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();    
    }    
  }
jashley2017 gravatar image jashley2017  ( 2020-07-22 10:01:15 -0500 )edit

Question Tools

Stats

Asked: 2020-07-21 11:23:02 -0500

Seen: 1,226 times

Last updated: Jul 21 '20