Initial commit

src/rideheight_sensing.cpp

@@ -0,0 +1,33 @@
+#include "rideheight_sensing/rideheight_sensing.h"
+
+#include <sensor_msgs/Range.h>
+
+namespace rh_sensing
+{
+RideheightSensing::RideheightSensing(ros::NodeHandle nh, ros::NodeHandle nh_private)
+  : nh{ nh }
+  , nh_private{ nh_private }
+  , vl6180x_pub{ nh.advertise<sensor_msgs::Range>("vl6180x", 10) }
+  , vl53l0x_pub{ nh.advertise<sensor_msgs::Range>("vl53l0x", 10) }
+{
+}
+
+void RideheightSensing::spin()
+{
+  sensor_msgs::Range vl6180x_msg, vl53l0x_msg;
+  vl6180x_msg.radiation_type = vl53l0x_msg.radiation_type = sensor_msgs::Range::INFRARED;
+  vl6180x_msg.field_of_view = vl53l0x_msg.field_of_view = 25.0f / 180 * M_PI;
+  vl6180x_msg.min_range = 0.005f;
+  vl6180x_msg.max_range = 0.1f;
+  vl53l0x_msg.min_range = 0.05f;
+  vl53l0x_msg.max_range = 1.0f;
+
+  ros::Rate loop_rate(10);
+  while (ros::ok())
+  {
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+}
+
+}  // namespace rh_sensing

src/rideheight_sensing_node.cpp

@@ -0,0 +1,15 @@
+#include "rideheight_sensing/rideheight_sensing.h"
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "rh_sensing");
+
+  ros::NodeHandle nh;
+  ros::NodeHandle nh_private("~");
+
+  rh_sensing::RideheightSensing sensing(nh, nh_private);
+
+  sensing.spin();
+
+  return 0;
+}