Using SentiUtils with C++
Preparations
Before you start integrating SentiUtil's data stream into C++, please make sure your system has SentiUtils and the Protobuf compiler installed.
To work with SentiUtil's Protobuf message definitions, please clone or download the SentiSystems senti_com ROS1 package, or senti-proto repository.
The ROS1 senti_com package requires your system to have a working ROS1 installation. See the official install instructions for Ubuntu.
senti_com - for usage with ROS1
- Setup a ROS workspace. Official tutorial
- Run
git clone --recurse-submodules https://gitlab.senti.no/senti/senti_com.git
from your<ros_workspace>/src/
folder. - Run
catkin_make
from<ros_workspace>
(catkin_make will generate the needed Protobuf files automatically). - Be sure to source
<ros_workspace>/devel/setup.bash
or addsource <ros_workspace>/devel/setup.bash
to your.bashrc
file. - Check that everything is working by running a
roscore
and starting theutils2ros
node.rosrun senti_com utils2ros
. - Sensor data should now be translated to ROS messages. Check
rostopic list
and print messages withrostopic echo /senti/<message>
.
senti_com - for usage with ROS2
- Setup a ROS2 workspace.
- Run
git clone -b ros2 --recurse-submodules https://gitlab.senti.no/senti/senti_com.git
from your<ros2_workspace>/src/
folder. - Run
colcon build
. - Be sure to source
<ros2_workspace>/install/setup.bash
or addsource <ros2_workspace>/install/setup.bash
to your.bashrc
file. - Start the
utils2ros
node.ros2 run senti_com utils2ros
- Sensor data should now be translated to ROS2 messages. Check
ros2 topic list
and print messages withros2 topic echo /senti/<message>
. Be vary of ROS2 default Quality-of-Service settings with respect to message loss etc.
senti-proto - for standalone usage
git clone https://gitlab.senti.no/senti/senti-proto.git
To generate the needed C++ files, proceed to run the following command
protoc --cpp_out=<insert location for output files> senti.proto
This should generate a senti.pb.h
and a senti.pb.cc
file, which should be placed in the same folder as your C++ software and included when compiling your software.
SentiMessageParser for C++
Note
The senti_com
ROS package is meant as a suggestion for how the SentiUtil's data stream should be converted to ROS. You are of course free to extend or change this example as you see fit. E.g by translating the data to other message types, or creating another system for publishing the data.
The senti_com ROS1 package contains a SentiUtils2Ros
class, which inherits from a SentiMessageParser
class. These two classes will make sure that the SentiUtil's data stream is handled, and callback functions are used to handle the seperate SentiSystems Protobuf messages.
Example usage is as follows:
#include "SentiUtils2Ros.hpp"
#include "proto/senti.pb.h"
#include "sensor_msgs/Imu.h"
#include "sensor_msgs/MagneticField.h"
#include "sensor_msgs/Temperature.h"
#include "geometry_msgs/TwistWithCovarianceStamped.h"
#include "std_msgs/Header.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "utils2ros");
ros::NodeHandle n;
SentiUtils2Ros utils2ros(n);
utils2ros.register_callback(SentiMessageID::IMU_MAG_ORIENTATION_MSG,
publish_imu_msg);
// Start the message receiving loop
utils2ros.run();
return 0;
}
The callback functions are functions which takes a std::string&
, and a pointer to the SentiUtils2Ros
class (SentiUtils2Ros*
) as input.
E.g:
void publish_imu_msg(const std::string& message, const SentiUtils2Ros* utils2ros)
{
SentiIMUMagOrientationMsg senti_imu_msg;
senti_imu_msg.ParseFromString(message);
sensor_msgs::Imu ros_imu_msg;
set_header_stamp(&ros_imu_msg.header, senti_imu_msg.header());
ros_imu_msg.linear_acceleration.x = senti_imu_msg.lin_acc()[0];
ros_imu_msg.linear_acceleration.y = senti_imu_msg.lin_acc()[1];
ros_imu_msg.linear_acceleration.z = senti_imu_msg.lin_acc()[2];
ros_imu_msg.linear_acceleration_covariance.fill(0);
ros_imu_msg.angular_velocity.x = senti_imu_msg.ang_vel()[0];
ros_imu_msg.angular_velocity.y = senti_imu_msg.ang_vel()[1];
ros_imu_msg.angular_velocity.z = senti_imu_msg.ang_vel()[2];
ros_imu_msg.angular_velocity_covariance.fill(0);
ros_imu_msg.orientation.x = senti_imu_msg.orientation()[0];
ros_imu_msg.orientation.y = senti_imu_msg.orientation()[1];
ros_imu_msg.orientation.z = senti_imu_msg.orientation()[2];
ros_imu_msg.orientation.w = senti_imu_msg.orientation()[3];
utils2ros->imu_pub.publish(ros_imu_msg);
sensor_msgs::Temperature ros_temperature_msg;
ros_temperature_msg.header = ros_imu_msg.header;
ros_temperature_msg.temperature = senti_imu_msg.temperature();
ros_temperature_msg.variance = 0;
utils2ros->imu_temperature_pub.publish(ros_temperature_msg);
sensor_msgs::MagneticField ros_mag_msg;
ros_mag_msg.header = ros_imu_msg.header;
ros_mag_msg.magnetic_field.x = senti_imu_msg.mag()[0] * GAUSS_TO_TESLA;
ros_mag_msg.magnetic_field.y = senti_imu_msg.mag()[1] * GAUSS_TO_TESLA;
ros_mag_msg.magnetic_field.z = senti_imu_msg.mag()[2] * GAUSS_TO_TESLA;
ros_mag_msg.magnetic_field_covariance.fill(0);
utils2ros->imu_mag_pub.publish(ros_mag_msg);
}