j.blanckenburg
/
rideheight-sensing
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Sense & publish VL6180x data

This commit is contained in:
jazzpi 2022-11-18 14:42:02 +01:00
parent c71d52a339
commit 9cec3672cf
3 changed files with 30 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
include/rideheight_sensing/rideheight_sensing.h
View File

@ -2,6 +2,8 @@
#include <ros/ros.h>
#include "vl6180x_platform.h"
namespace rh_sensing
{
class RideheightSensing
@ -17,6 +19,9 @@ private:
ros::Publisher vl6180x_pub;
ros::Publisher vl53l0x_pub;
VL6180xDev_t vl6180x_dev;
VL6180x_RangeData_t vl6180x_range;
};
} // namespace rh_sensing

2
include/vl6180x_glue.h
View File

@ -1,3 +1,5 @@
#pragma once
#define VL6180X_I2C_ADDR 0x29
void VL6180x_glue_init(const char* i2c_dev_path);

37
src/rideheight_sensing.cpp
View File

@ -1,21 +1,25 @@
#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) }
{
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-3");
VL6180x_InitData(vl6180x_dev);
VL6180x_Prepare(vl6180x_dev);
}
void RideheightSensing::spin()
{
void RideheightSensing::spin() {
sensor_msgs::Range vl6180x_msg, vl53l0x_msg;
vl6180x_msg.radiation_type = vl53l0x_msg.radiation_type = sensor_msgs::Range::INFRARED;
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;
@ -23,11 +27,16 @@ void RideheightSensing::spin()
vl53l0x_msg.max_range = 1.0f;
ros::Rate loop_rate(10);
while (ros::ok())
{
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
} // namespace rh_sensing