43 lines
1.3 KiB
C++
43 lines
1.3 KiB
C++
#include "rideheight_sensing/rideheight_sensing.h"
|
|
|
|
#include "vl6180x_api.h"
|
|
#include "vl6180x_glue.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)} {
|
|
VL6180x_glue_init("/dev/i2c-8");
|
|
VL6180x_InitData(vl6180x_dev);
|
|
VL6180x_Prepare(vl6180x_dev);
|
|
}
|
|
|
|
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()) {
|
|
VL6180x_RangePollMeasurement(vl6180x_dev, &vl6180x_range);
|
|
if (vl6180x_range.errorStatus == 0) {
|
|
vl6180x_msg.header.stamp = ros::Time::now();
|
|
vl6180x_msg.range = vl6180x_range.range_mm / 1000.0f;
|
|
vl6180x_pub.publish(vl6180x_msg);
|
|
}
|
|
ros::spinOnce();
|
|
loop_rate.sleep();
|
|
}
|
|
}
|
|
|
|
} // namespace rh_sensing
|