Code listing & code review

Marco Maissan
0949830

Rotterdam University of Applied Sciences

Festo BV

supervised by:
G. Maas
W. Volders

June 26, 2021

1 Code listing
This project mainly consisted of the following tasks:

• Conducting research for hardware;

• Designing and mounting hardware on the Robotino;

• Designing and building software architectures using launch files;

• Compiling pre-made open source packages;

• Writing and tuning package configuration files.

Writing code by hand had a significantly lower priority compared to other tasks. There are
two packages that have been made by hand, of which only one has been used in the end.
Other than that, mostly open source packages are used. This code listing contains only the
files that have been written and/or modified manually. They are described in the following
tables.

1
Files on Robotino on-board computer Description
src/robotino mapper/cfg/costmap common params.yaml Parameters that are being
used in both the local and
global costmap.
src/robotino mapper/cfg/global costmap params.yaml Parameters for the global
costmap.
src/robotino mapper/cfg/global planner params.yaml Parameters for the global
planner.
src/robotino mapper/cfg/local costmap params.yaml Parameters for local costmap.
src/robotino mapper/cfg/rtabmap config.yaml RTAB-MAP configuration
file. This contains the pa-
rameters for the RTAB-MAP
ROS configuration (e.g. sub-
scribing to topics), as well
as the internal RTAB-MAP
functionality parameters (e.g.
computation performance
parameters).
src/robotino mapper/cfg/teb local planner params.yaml Parameters for the Timed
Elastic Band (TEB) local
planner.
src/robotino mapper/launch/mapper.launch Launch file that starts map-
ping/localization.
src/robotino node/include/RobotinoNode.h Header file for CPP Robotino
node.
src/robotino node/src/Robotino.cpp Code that manages IR sensor
readings and omnidirectional
drive handling
src/robotino node/src/RobotinoNode.cpp Code that starts manages the
ROS topic subscriptions and
publishers. Makes use of
Robotino.cpp file.

2
Files on Nvidia Jetson Xavier NX Description
(deprecated)
Header file for Range Filter.
src/range filter/include/RangeFilter.h
(deprecated)
Custom node that was used
src/range filter/src/RangeFilter.cpp
to clear obstacles from the
Costmap. It publishes a cir-
cular point cloud around the
Robotino, which is used was
used for the raytracing clear-
ing function. This is dis-
missed in favor of the spa-
tio temporal voxel layer.
(deprecated)
File that starts up the
src/range filter/src/RangeFilterNode.cpp
RangeFilter.cpp executable
and makes all ROS connec-
tions.
src/robotino preprocessor/launch/include/zed config.yaml Configuration file for ZED 2
stereo camera.
src/robotino preprocessor/launch/preprocessor.launch File that launches the stereo
camera, RVIZ and Voxel Fil-
ters.
src/robotino preprocessor/rviz/robotino.rviz The GUI configuration for
RVIZ.
src/robotino preprocessor/urdf/robotino.urdf.xacro The URDF file that is used
to build a TF tree model of
the Robotino and its addi-
tional hardware.

Files for web interface Description

index.html The HTML for the Robotino control page.
rosconnector.js The JavaScript file that handles all WebSocket connections
and ROS interfacing functionality.
styles.css Few styling adjustments.

3
2 Code review
The code given in the code listing has been reviewed by Florèn van Olden and Oscar Fokker.
Both are engineers at Festo and both have hands-on experience with Robot Operating System
(ROS). The code review is executed by making a pull request to a code review branch, then
merging it back into the master branch after fixing the comments. Its structure is graphically
represented in figure 2. The code has been checked for style errors, optimizations and
unclarities.

Figure 2: Code review structure

3 Code review Robotino

The next pages include the code review of the code that runs on the Robotino.

4
6/17/2021 This is the official code review by Oscar Fokker and Floren van Olden (!3) · Merge requests · Marco Maissan / robotino-robotino · GitLab

Open Created 5 hours ago by Marco Maissan

This is the official code review by Oscar Fokker and

Floren van Olden

This merge request is used for code review purposes and will be merged back into the development branch
afterwards.

Approval is optional

Squash commits

14 commits and 1 merge commit will be added to code_review.

Marco Maissan @marcomaissan requested review from @Ozzynator 5 hours ago

Marco Maissan @marcomaissan assigned to @marcomaissan 5 hours ago

Oscar Fokker @Ozzynator started a thread on the diff 5 hours ago

41 + observation_persistence: 0.0 #default 0, use all measurements

taken during now-value, 0=latest

42 + clear_after_reading: true #default false, clear the buffer

after the layer gets readings from it

43 + filter: "voxel" #default passthrough, apply

"voxel", "passthrough", or no filter to sensor data, recommended to
have at one filter on

44 + voxel_min_points: 0 #default 0, minimum points per

voxel for voxel filter

45 + zed_clear:

46 + data_type: PointCloud2

47 + topic: obstacles

48 + marking: false

49 + clearing: true

50 + min_z: 0.1 #default 0, meters

51 + max_z: 7.0 #default 10, meters

52 + vertical_fov_angle: 1.2 #default 0.7, radians

53 + horizontal_fov_angle: 1.91 #default 1.04, radians

54 + decay_acceleration: 1.0 #default 0, 1/s^2. If laser

scanner MUST be 0

55 + model_type: 0 #default 0 (depth camera). Use 1

for 3D Lidar

56 +

Oscar Fokker @Ozzynator · 5 hours ago Maintainer

why the blank space

Reply…
src/robotino_mapper/cfg/costmap_common_params.yaml

Oscar Fokker @Ozzynator started a thread on the diff 5 hours ago

72 + myRequest.perform();

73 +

74 + //split the string

75 + std::string output = os.str();

76 + std::vector<std::string> results;
https://gitlab.com/marcomaissan/robotino-robotino/-/merge_requests/3 1/5
6/17/2021 This is the official code review by Oscar Fokker and Floren van Olden (!3) · Merge requests · Marco Maissan / robotino-robotino · GitLab
76 + std::vector<std::string> results;

77 + boost::split(results, output, [](char c){return c == '\n';});

78 +

79 + //convert string array to float array with sensor values

80 + float point_data[9];

81 + for(int i = 1; i < 10; i++){

82 + point_data[i-1] = std::stof(results[i]);

83 + }

84 +

85 + // Set dimensions of point cloud with nine IR sensors

86 + cloud_msg.height = 1;

87 + cloud_msg.width = 9;

Oscar Fokker @Ozzynator · 5 hours ago Maintainer

missing spacing

Reply…
src/robotino_node/src/RobotinoNode.cpp

Oscar Fokker @Ozzynator started a thread on the diff 5 hours ago

Last updated
by
Floren van Olden
4 hours ago

104 + sensor_msgs::PointCloud2Iterator<uint8_t>
iterator_b(cloud_msg, "b");

105 + //Fill the PointCloud2

106 + //Fill iterator data. Coordinate is defined as [x=cos(i*PI/9)*

(range_measurement+robotino_radius), y=sin(...)*(...)]

107 + //If the value is maximum, change it to NaN so it will not be

detected as obstacle

108 + //Color is white.

109 + for(size_t i=0; i<9; ++i, ++iterator_x, ++iterator_y,

++iterator_r, ++iterator_g, ++iterator_b) {

110 + *iterator_r = 255;

111 + *iterator_g = 255;

112 + *iterator_b = 255;

113 + if(point_data[i] <= 0.4f){

114 + *iterator_z = 0;

115 + *iterator_x = cos(i * STEP_SIZE) * (point_data[i] +

ROBOTINO_RADIUS);

116 + *iterator_y = sin(i * STEP_SIZE) * (point_data[i] +

ROBOTINO_RADIUS);

117 + }else{

118 + *iterator_x = *iterator_y = *iterator_z =

std::numeric_limits<float>::quiet_NaN();

119 + }

Oscar Fokker @Ozzynator · 5 hours ago Maintainer

spacing for the followin lines seems of

Floren van Olden @florenvanolden · 4 hours ago Maintainer

Kijk eens naar het codemaid project voor dit soort basis lelijkheden.

Reply…
src/robotino_node/src/RobotinoNode.cpp

Marco Maissan @marcomaissan requested review from @florenvanolden and removed

review request for @Ozzynator 4 hours ago

Floren van Olden @florenvanolden started a thread on the diff 4 hours ago

16 16

17 17 <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap"

output="screen" args="$(arg rtabmap_args)">

18 18 <!-- delete_db_on_start -->

https://gitlab.com/marcomaissan/robotino-robotino/-/merge_requests/3 2/5
6/17/2021 This is the official code review by Oscar Fokker and Floren van Olden (!3) · Merge requests · Marco Maissan / robotino-robotino · GitLab

Floren van Olden @florenvanolden · 4 hours ago Maintainer

Mis de toelichting, waarom je welke modules laad en wat de dependancies zijn.

Reply…
src/robotino_mapper/launch/mapper.launch

Floren van Olden @florenvanolden started a thread on the diff 4 hours ago

1 + TebLocalPlannerROS:

2 + # Trajectory

3 + teb_autosize: True

4 + dt_ref: 0.6

5 + dt_hysteresis: 0.1

6 + max_samples: 100

7 + global_plan_overwrite_orientation: True

8 + allow_init_with_backwards_motion: False

9 + max_global_plan_lookahead_dist: 5.0

10 + global_plan_viapoint_sep: -1

11 + global_plan_prune_distance: 1

12 + exact_arc_length: False

13 + feasibility_check_no_poses: 5

14 + publish_feedback: False

15 +

16 + # Robot

Floren van Olden @florenvanolden · 4 hours ago Maintainer

Kun je deze parameters straffeloos aanpassen zonder dat dit effect heeft op andere modules?
Of vind ik de acceleratie en velocity parameters ook nog ergens anders, zo ja graag in
comment

Reply…
src/robotino_mapper/cfg/teb_local_planner_params.yaml

Floren van Olden @florenvanolden started a thread on the diff 4 hours ago

9 + max_global_plan_lookahead_dist: 5.0

10 + global_plan_viapoint_sep: -1

11 + global_plan_prune_distance: 1

12 + exact_arc_length: False

13 + feasibility_check_no_poses: 5

14 + publish_feedback: False

15 +

16 + # Robot

17 + max_vel_x: 0.35

18 + max_vel_x_backwards: 0.1

19 + max_vel_y: 0.05

20 + max_vel_theta: 0.15

21 + acc_lim_x: 0.05

22 + acc_lim_y: 0.05

23 + acc_lim_theta: 0.05

24 + min_turning_radius: 0.0

Floren van Olden @florenvanolden · 4 hours ago Maintainer

9 unresolved threads
Waarom niet opgegeven? Of kan hij echt om zijn as draaien?

src/robotino_mapper/cfg/teb_local_planner_params.yaml
Reply…

Floren van Olden @florenvanolden started a thread on the diff 4 hours ago

https://gitlab.com/marcomaissan/robotino-robotino/-/merge_requests/3 3/5
6/17/2021 This is the official code review by Oscar Fokker and Floren van Olden (!3) · Merge requests · Marco Maissan / robotino-robotino · GitLab

src/robotino_mapper/cfg/rtabmap_config.yaml

8 + subscribe_user_data: false

9 + subscribe_odom_info: false

10 + database_path: "$(find robotino_mapper)/db/database_rtabmap.db"

11 + map_frame_id: "map"

12 + odom_frame_id: "odom" # odometry from odom msg to

have covariance - Remapped by launch file

13 + frame_id: "base_link"

14 + odom_tf_angular_variance: 0.001 # If TF is used to get odometry,

this is the default angular variance

15 + odom_tf_linear_variance: 0.001 # If TF is used to get odometry,

this is the default linear variance

16 + tf_delay: 0.10

17 + publish_tf: true # Set to false if fusing

different poses (map->odom) with UKF

18 + odom_sensor_sync: true

19 + wait_for_transform: false

20 + #wait_for_transform_duration: 0.1

21 + queue_size: 10

22 + scan_normal_k: 0

23 + rgb_topic: "/zed_wrapper/rgb/image_rect_color"

Floren van Olden @florenvanolden · 4 hours ago Maintainer

Welke wrapper gebruik je daadwerkelijk voor je SLAM?

Reply…

Floren van Olden @florenvanolden started a thread on the diff 4 hours ago

src/robotino_mapper/cfg/costmap_common_params.yaml
42 + clear_after_reading: true #default false, clear the buffer
after the layer gets readings from it

43 + filter: "voxel" #default passthrough, apply

"voxel", "passthrough", or no filter to sensor data, recommended to
have at one filter on

44 + voxel_min_points: 0 #default 0, minimum points per

voxel for voxel filter

45 + zed_clear:

46 + data_type: PointCloud2

47 + topic: obstacles

48 + marking: false

49 + clearing: true

50 + min_z: 0.1 #default 0, meters

51 + max_z: 7.0 #default 10, meters

52 + vertical_fov_angle: 1.2 #default 0.7, radians

53 + horizontal_fov_angle: 1.91 #default 1.04, radians

54 + decay_acceleration: 1.0 #default 0, 1/s^2. If laser

scanner MUST be 0

55 + model_type: 0 #default 0 (depth camera). Use 1

for 3D Lidar

56 +

57 + ir_mark:

Floren van Olden @florenvanolden · 4 hours ago Maintainer

Wat is het verschil tussen je IR clear en mark?

Reply…

Floren van Olden @florenvanolden started a thread on the diff 4 hours ago

src/robotino_node/include/RobotinoNode.h

1 + #ifndef ROBOTINONODE_H_

2 + #define ROBOTINONODE_H

3 +

4 + class RobotinoNode{
https://gitlab.com/marcomaissan/robotino-robotino/-/merge_requests/3 4/5
6/17/2021 This is the official code review by Oscar Fokker and Floren van Olden (!3) · Merge requests · Marco Maissan / robotino-robotino · GitLab
4 + class RobotinoNode{

5 + public:

6 + sensor_msgs::PointCloud2 getIRSensorData();

7 + void drive(const geometry_msgs::Twist::ConstPtr& msg);

8 +

9 + private:

10 + const float STEP_SIZE = 0.69813170079f;

Floren van Olden @florenvanolden · 4 hours ago Maintainer

Waar komt deze waarde vandaan, is het bewust gekozen en waarom dan deze stapgrote?

Reply…

https://gitlab.com/marcomaissan/robotino-robotino/-/merge_requests/3 5/5
4 Code review Nvidia Jetson Xavier NX
The next pages include the code review of the code that runs on the Nvidia Jetson Xavier
NX.

10
6/17/2021 Code review by Floren and Oscar (!1) · Merge requests · Marco Maissan / robotino-jetson · GitLab

Open Created 4 hours ago by Marco Maissan

Code review by Floren and Oscar

Approval is optional

Squash commits

24 commits and 1 merge commit will be added to code_review.

Marco Maissan @marcomaissan requested review from @florenvanolden 4 hours ago

Marco Maissan @marcomaissan assigned to @marcomaissan 4 hours ago

2 unresolved threads

Floren van Olden @florenvanolden started a thread on the diff 3 hours ago

src/robotino_preprocessor/launch/preprocessor.launch

9 + <arg name="pos_y" default="0.0"/>

10 + <arg name="pos_z" default="0.34"/>

11 + <arg name="rot_x" default="0.0"/>

12 + <arg name="rot_y" default="0.0"/>

13 + <arg name="rot_z" default="0.0"/>

14 + <!-- Start zed wrapper -->

15 + <node name="zed_wrapper" pkg="zed_wrapper" type="zed_wrapper_node"

output="screen" required="true">

16 + <rosparam file="$(find
robotino_preprocessor)/launch/include/zed_config.yaml"
command="load"/>

17 + </node>

18 + <!-- Run a VoxelGrid filter to clean NaNs and downsample the data -->

19 + <node pkg="nodelet" type="nodelet" name="obstacle_manager"

args="manager" output="screen"/>

20 + <node pkg="nodelet" type="nodelet" name="voxel_grid_1" args="load

pcl/VoxelGrid obstacle_manager" output="screen">

21 + <remap from="~input" to="/zed_wrapper/point_cloud/cloud_registered"/>

22 + <remap from="~output" to="/voxel_1"/>

23 + <rosparam subst_value="true">

24 + filter_field_name: x

Floren van Olden @florenvanolden · 3 hours ago Maintainer

probeer codemaid eens voor je opmaak, logischer om hier wat tabjes toe te voegen.

Reply…

Floren van Olden @florenvanolden started a thread on the diff 3 hours ago

src/robotino_preprocessor/launch/include/zed_config.yaml
21 + serial_number: 0

22 + resolution: 2 #
'0': HD2K, '1': HD1080, '2': HD720, '3': VGA

23 + grab_frame_rate: 15 #
Frequency of frame grabbing for internal SDK operations

24 + gpu_id: -1

25 + base_frame: 'base_link' #
must be equal to the frame_id used in the URDF file

26 + verbose: false #
Enable info message by the ZED SDK

27 + svo_compression: 2 #
`0`: LOSSLESS, `1`: AVCHD, `2`: HEVC

https://gitlab.com/marcomaissan/robotino-jetson/-/merge_requests/1 1/2
6/17/2021 Code review by Floren and Oscar (!1) · Merge requests · Marco Maissan / robotino-jetson · GitLab

28 + self_calib: true #
enable/disable self calibration at starting

29 + camera_flip: false

30 +

31 + video:

32 + img_downsample_factor: 1.0 #
Resample factor for images [0.01,1.0] The SDK works with native image
sizes, but publishes rescaled image.

33 + extrinsic_in_camera_frame: true # if
`false` extrinsic parameter in `camera_info` will use ROS native
frame (X FORWARD, Z UP) instead of the camera frame (Z FORWARD, Y
DOWN) [`true` use old behavior as for version < v3.1]

34 +

35 + depth:

36 + quality: 1 #
'0': NONE, '1': PERFORMANCE, '2': QUALITY, '3': ULTRA

Floren van Olden @florenvanolden · 3 hours ago Maintainer

heb je hiermee geexperimenteerd?

Reply…

https://gitlab.com/marcomaissan/robotino-jetson/-/merge_requests/1 2/2