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

Sense & publish VL6180x data

Browse Source
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 <ros/ros.h>
#include "vl6180x_platform.h"
namespace rh_sensing namespace rh_sensing
{ {
class RideheightSensing class RideheightSensing
@ -17,6 +19,9 @@ private:
ros::Publisher vl6180x_pub; ros::Publisher vl6180x_pub;
ros::Publisher vl53l0x_pub; ros::Publisher vl53l0x_pub;
VL6180xDev_t vl6180x_dev;
VL6180x_RangeData_t vl6180x_range;
}; };
} // namespace rh_sensing } // namespace rh_sensing

2
include/vl6180x_glue.h
View File

@ -1,3 +1,5 @@
#pragma once #pragma once
#define VL6180X_I2C_ADDR 0x29 #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 "rideheight_sensing/rideheight_sensing.h"
#include "vl6180x_api.h"
#include "vl6180x_glue.h"
#include <sensor_msgs/Range.h> #include <sensor_msgs/Range.h>
namespace rh_sensing namespace rh_sensing {
{ RideheightSensing::RideheightSensing(ros::NodeHandle nh,
RideheightSensing::RideheightSensing(ros::NodeHandle nh, ros::NodeHandle nh_private) ros::NodeHandle nh_private)
: nh{ nh } : nh{nh}, nh_private{nh_private},
, nh_private{ nh_private } vl6180x_pub{nh.advertise<sensor_msgs::Range>("vl6180x", 10)},
, vl6180x_pub{ nh.advertise<sensor_msgs::Range>("vl6180x", 10) } vl53l0x_pub{nh.advertise<sensor_msgs::Range>("vl53l0x", 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; 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.field_of_view = vl53l0x_msg.field_of_view = 25.0f / 180 * M_PI;
vl6180x_msg.min_range = 0.005f; vl6180x_msg.min_range = 0.005f;
vl6180x_msg.max_range = 0.1f; vl6180x_msg.max_range = 0.1f;
@ -23,11 +27,16 @@ void RideheightSensing::spin()
vl53l0x_msg.max_range = 1.0f; vl53l0x_msg.max_range = 1.0f;
ros::Rate loop_rate(10); 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(); ros::spinOnce();
loop_rate.sleep(); loop_rate.sleep();
} }
} }
} // namespace rh_sensing } // namespace rh_sensing