Examples for using Makeblock sensors

3

Making available data from Makeblock or any other sensors for ROS is simple. Here is an example for ultasonic distance sensor (not from Makeblock, but for this example it doesn’t matter):

Code for CORE2-ROS, SBC part (eg. Raspberry Pi 3) with Husarion’s Linux image:

#include <ros/ros.h>
#include <sensor_msgs/Range.h>
 
float dist = 0;
void dist_callback(const sensor_msgs::Range &range) {
    dist = range.range;
}
int main(int argc, char **argv) {
    ros::init(argc, argv, "distance_sensor");
    ros::NodeHandle n("~");
    ros::Subscriber dist_sub = n.subscribe("/range", 10, dist_callback);
    ros::Rate loop_rate(200);
    while (ros::ok()) {
        ros::spinOnce();
        std::cout << "Distance: " << dist << std::endl;
        loop_rate.sleep();
    }
}

Code for CORE2-ROS, real-time part (will run on STM32 microcontoller):

#include "hFramework.h"
#include "hCloudClient.h"
#include <ros.h>
#include "tf/tf.h"
#include <DistanceSensor.h>
#include "sensor_msgs/Range.h"
using namespace hModules;
using namespace hFramework;

DistanceSensor sens(hSens2);

ros::NodeHandle nh;
sensor_msgs::Range range;
ros::Publisher range_pub("/range", &range);

void hMain()
{
	platform.begin(&RPi);
	nh.getHardware()->initWithDevice(&platform.LocalSerial);
	nh.initNode();
	LED1.on();
	nh.advertise(range_pub);

	while (true) {
            int dist = sens.getDistance();
            range.range = (float)dist;
            range_pub.publish(&range);
            nh.spinOnce();
            LED1.toggle();
            sys.delay(10); // 10 ms
        }
}