No Description
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

scanerapi_sample_ros_receive.cpp 1.6KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344
  1. #include "ros/ros.h"
  2. #include "std_msgs/String.h"
  3. #include "ScanerAPI/scanerAPI_DLL_C.h" //SCANeRTM API: C language Functions
  4. #include "ScanerAPI/ScanerAPImessagesNetwork.h" //SCANeRTM API: Network utils
  5. #include "ScanerAPI/ScanerAPImessagesShm.h"
  6. #include <sstream>
  7. int main(int argc, char **argv)
  8. {
  9. std_msgs::String msg;
  10. std::stringstream ss;
  11. float speed=0.0;
  12. ros::init(argc, argv, "scanerAPI_sample_ros_receive");
  13. ros::NodeHandle p;
  14. Process_Init(argc, argv); //SCANeRTM API: Process initialization
  15. //Instantiation of the publisher, it will send data msg to the calculator
  16. ros::Publisher speed_pub = p.advertise<std_msgs::String>("speed_sensed", 100000);
  17. //Definition on a vehicle interface to get any data from the vehicle
  18. DataInterface* vehicle_0_in = Com_declareInputData(NETWORK_IVEHICLE_VEHICLEUPDATE, 0);
  19. while (ros::ok())
  20. {
  21. Process_Wait(); //SCANeRTM API: Frequency synchronization
  22. Process_Run(); //SCANeRTM API: Run the process
  23. if (Process_GetState() == PS_RUNNING) //SCANeRTM API: The simulation is running
  24. {
  25. Com_updateInputs(UT_AllData); //SCANeRTM API: Update input data (data to read)
  26. speed=Com_getFloatData(vehicle_0_in, "speed[0]")*3.6; //SCANeRTM API: Read vehicle 0’s speed on X;//SCANeRTM API: Read vehicle 0’s speed.
  27. ss << speed;
  28. msg.data = ss.str();
  29. ss.str("");
  30. //Here we show the value on the terminal to check it first before sending it
  31. ROS_INFO("%s", msg.data.c_str());
  32. //Here we publish the value to the calculator node
  33. speed_pub.publish(msg);
  34. ros::spinOnce();
  35. }
  36. }
  37. Process_Close(); //SCANeRTM API: Clean way to stop the SCANeRTM module
  38. return 0;
  39. }