Examples for using Makeblock sensors

Are there any examples or guides/tutorials to usse the Core2 with Makeblock sensors?
I found nothing in the tutorials or in GitHub.

Hi Michael,

Makeblock sensors libraries (GitHub - Makeblock-official/Makeblock-Libraries: Arduino Library for Makeblock Electronic Modules, learn more from Makeblock official website) use Arduino framework. We are developing Arduino compatibility layer for hFramework (open source, low-level programming framework for CORE2). Source code for this “compatibility layer” you can find here: hFramework/ports/stm32/include/Arduino.h at master · husarion/hFramework · GitHub . We are still developing this in our private repo and will make it public, shortly after you get your CORE2.

Names of CORE2 interfaces accessible using Arduino framework you can find here:

When full Arduino port will be ready, all you will need to start using Makeblock sensors is to add Makeblock library to your project.

You can use Makeblock motors even now, like this:

#include "hFramework.h"

void hMain()
{
	Serial.init(115200);
	sys.setLogDev(&Serial);
	sys.setSysLogDev(&devNull);

	while (1) {
		printf("\r\nClick q or a button: ");
		char c = Serial.getch();
		if (c == 'q') {
			hMot1.rotRel(180);
			printf("\r\nRotate motor 1, 180 encoder ticks right");
		}
		if (c == 'a') {
			hMot1.rotRel(-180);
			printf("\r\nRotate motor 1, 180 encoder ticks left");
		}
	}
}

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
        }
}

Hi Dominik
Thanks for your answer.
This makes some things more clear to me, e.g. the Core2 board acts as a ROS node (I assume via rosserial) and communicates via ROS to the SBC. My first thought was that SBC and Core2 has a serial connection (with an own protocol) and the SB does the ROS part allone. Seems I was wrong, but would that also be possible?
Somewhere in the documentation I found that hSens2 was a I2C variable so the DistanceSensor seems to use the I2C protocol. What was the DistanceSensor for a library and how must it be included in the Developement Environment? What is the process/workflow from start to download?
Perhaps that would be another tutorial ;-).
Sorry for my questions but I am waiting for my ordered parts and try to prepare something to start immediate when they arrive.

Thanks again for your patience.

Michael

Hi Michael,

both CORE2 and SBC part run ROS software. We spent a lot of time on integrating ROS and our libraries, and it works pretty well :slight_smile:

it looks like you found library for LEGO Distance Sensor: hSensors/Lego_Ultrasonic.cpp at master · husarion/hSensors · GitHub . It uses I2C. Some libraries for sensors are available directly from a level of Web IDE without adding additional library files to your projects. Just include a header file in your project.

Here is a list of libaries available directly from a level of Web IDE right now.
https://husarion.com/core2/api_reference/namespaceh_sensors.html
https://husarion.com/core2/api_reference/classh_modules_1_1_distance_sensor.html
https://husarion.com/core2/api_reference/classh_modules_1_1_m_p_u9250.html

You can of course add additional libaries - just add source and header files to the project.