The main solution package created for this is the smb_highlevel_controller which was created with the following command:

catkin_create_pkg smb_highlevel_controller

Setup

As the exercise requires the use of ROS in gazebo simulation, it is assumed that the computer is properly setup with ros-melodic-desktop-full or ros-noetic-desktop-full installation. Once ROS is installed, you can run the following command to create a Catkin workspace:

sudo apt-get install python3-catkin-tools
mkdir -p catkin_ws/src
cd ~/catkin_ws
catkin build

# Automatically source setup.bash for convenience.
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

This ROS stack requires some dependencies which can be installed with the following command:

sudo apt install -y ros-<distro>-hector-gazebo-plugins \
                    ros-<distro>-velodyne \
                    ros-<distro>-velodyne-description \
                    ros-<distro>-velodyne-gazebo-plugins \
                    ros-<distro>-pointcloud-to-laserscan \
                    ros-<distro>-twist-mux

where <distro> can be either melodic or noetic.

Once everything is fully setup, you can clone the package into the catkin_ws/src directory and build the entire package:

cd ~/catkin_ws/src
git clone https://github.com/NelsenEW/eth-zurich-solution
cd ..
catkin build

Exercise 1

This exercise is based on lecture 1.

Run the launch file with the following command:

roslaunch smb_highlevel_controller smb_highlevel_controller.launch

Output

Gazebo with SMB and teleop twist keyboard

Command line

• To control smb robot in gazebo through command line (press tab for autocompletion):

  rostopic pub /cmd_vel geometry_msgs/Twist '[0.5,0,0]' '[0,0,0]'

smb_highlevel_controller.launch

• The world file argument is hardcoded as follow:

  <arg name="world_file" value="/usr/share/gazebo-9/worlds/robocup14_spl_field.world"/>

• To launch the teleop keyboard in a new terminal, set the launch-prefix to xterm -e

Exercise 2

This exercise is based on lecture 2.

Run the launch file with the following command:

roslaunch smb_highlevel_controller smb_highlevel_controller.launch

The solution package template is based on ros_best_practices for python

Output

The solution output should be as follow:

Rviz with laserscan, terminal with output and gazebo

pointcloud_to_laserscan

|

As can be seen from the rqt_graph, the pointcloud_to_laserscan node is subscribing to /rslidar_points which is a PointCloud2 message and /tf and converts it into a LaserScan topic /scan.

Files

default_parameters.yaml:

• consist of parameters that are passed to the launch file.

smb_highlevel_controller:

• Initialize the smb_highlevel_controller node

SmbHighlevelController.py

• Implementation of the class method including fetch parameters from launch
• Subscribe to topics name based on parameters server
• Implementation of callback method such as scanCallback and pclCallback.

smb_highlevel_controller.rviz:

• contains rviz file format which were created by running rviz seperately, adding the required display, and saving it into the rviz file.

smb_highlevel_controller.launch:

• Add <rosparam> to load default_parameters.yaml to parameter server.
• Add node to launch the smb_highlevel_controller.py script.

CMakeLists.txt:

• Add find_package and catkin_package to find libraries such as rospy and sensor_msgs.
• Install python executable based on the project name with catkin_install_python .

package.xml

• Add depend for the dependencies which are rospy, sensor_msgs and smb_gazebo

Exercise 3

Note: Change smb_common package to smb_common_v2 package

This exercise is based on lecture 3.

Run the launch file with the smb_highlevel_controller with the following command:

roslaunch smb_highlevel_controller smb_highlevel_controller.launch

Output

The solution output should be as follow:

Rviz with marker visualization indicate with the green color ball and tf marker, terminal with printed output such as the angle , and smb is heading towards the pillar in gazebo

Files

CMakeLists.txt and package.xml:

• Add dependencies such as geometry_msgs, tf2_ros, and visualization_msgs package.

SmbHighlevelController.py:

• Import geometry_msgs, tf2_ros, and visualization_msgs package.
• Add two publisher for topics visualization_marker and cmd_vel during initialization.
• Create a goal_pose of type geometry_msgs::PoseStamped which is the pillar from the lidar reading with respect to the rslidar frame.
• Create TF listerner and TF buffer to transform the goal_pose from the rslidar frame to odom on transform_odom.
• Utilize a P controller from the error angle to drive the error to zero on move_to_goal, the x velocity is set to constant without P controller to ensure that the SMB hits the pillar.
• Publish a visualization marker on vis_marker_publish that can be displayed in Rviz.

smb_highlevel_controller.launch:

• Change the world argument value to "$(find smb_highlevel_controller)/world/singlePillar.world"
• Add two arguments under laser_scan_min_height and laser_scan_max_height to -0.2 and 1.0 respectively.
• Remove the teleop_twist_keyboard node from the launch.

smb_highlevel_controller.rviz:

• Add Marker display to visualize the pillar marker indicated with green color ball.

Exercise 4

This exercise is based on lecture 3 and lecture 4.

This exercise requires the use of rqt_multiplot. Run the following command to install rqt_multiplot: sudo apt install -y ros-<distro>-rqt-multiplot

where <distro> can be either melodic or noetic based on your computer ROS_DISTRO.

Simulation

The simulation can be run with the following command: roslaunch smb_highlevel_controller smb_highlevel_controller.launch

EKF Localization Node

To understand the EKF Localization Node, open another terminal, then open it with rqt_graph.

The output is the following:

ekf_localization node in rqt_graph

As can be seen from the graph, the ekf localization subscribes to /imu/data and /smb_velocity_controller/odom topics and publishes /odometry/filtered topic by applying extended kalman filter. In this case, the topic will be displayed in both rqt_multiplot and rviz.

Plot of simulation x/y-plane

The solution output should be as follow:

Plot of x/y-plane that is taken by SMB (Kp = 30, x_vel = 3 m/s) until it hits the pillar on rqt_multiplot

Recorded (rosbag)

ROS Topic inside smb_navigation.bag

To get all the topics and messages inside the rosbag, run the following command:

rosbag info smb_navigation.bag

The solution should be as follow:

To run the recorded rosbag, use the following command:

roslaunch smb_highlevel_controller ekf_localization.launch

Plot of recorded x/y-plane

The solution output should be as follow:

Plot of x/y-plane plot that is taken by SMB until the rosbag recording ends

Visualization of 3D point cloud and TF marker in Rviz

The 3D point cloud as well as smb_top_view frame can be visualize in rviz:

3D lidar point cloud and smb_top_view frame visualize in rviz

The smb_top_view frame will move according to the base_link frame. As such, the smb_top_view is moving together with the robot in rviz when the rosbag is played.

Files

smb_navigation.bag:

• Contains 59.7 seconds of a recorded simulation.
• The size of the bag is 158.9 MB with total messages of 1545.
• The topics recorded are /imu/data, join_states, rslidar_points, and smb_velocity_controller/odom

xy_multiplot.xml:

• Create an x/y-plane plot of the smb based on the output of the ekf_localization node which is /odometry/filtered with type nav_msgs/Odometry.

ekf_localization.rviz:

• Display TF, PointCloud2, and RobotModel of the smb

smb_highlevel_controller.launch:

• Add rqt_multiplot node with xy_multiplot.xml to plot the path of smb in x/y plane.

ekf_localization.launch:

• Refer from control.launch file that is located on the smb_control package.
• Add ekf_robot_localization node and load the required parameters from the localization.yaml
• Add smb_robot_state_publisher to publish state of the robot to tf2 that is visualize in rviz.
• Create a frame called smb_top_view with static_transform_publisher node which is 2 meters above the base_link frame.
• Add rosbag node to play rosbag with full speed or half speed.
• Launch rviz with ekf_localization.rviz configuration.
• Add rqt_multiplot node with xy_multiplot.xml to plot the path of smb in x/y plane.

Exercise 5

This exercise is based on lecture 4.

Manual Service call

The service name /startService is defined inside default_parameters.yaml.

Run the launch file with the following command:

roslaunch smb_highlevel_controller smb_highlevel_controller.launch

To start the robot, run the following command on another terminal:

rosservice call /startService "data: true"

Alternatively you can run the robot during the startup with the following command:

roslaunch smb_highlevel_controller smb_highlevel_controller.launch start_robot:="true"

To stop the robot manually from colliding, open another terminal and run the following command:

rosservice call /startService "data: false"

The robot can always continue its path by calling the service by setting the data to true.

Automatic emergency

Prior collision

Run the launch file with the following command:

roslaunch smb_highlevel_controller smb_highlevel_controller.launch start_robot:="true" auto_emergency:="true"

By default, the robot will stop before hitting the pillar with a distance of max_distance_to_pillar from the robot's lidar that is specified in the default_parameters.yaml.

The solution output should be as follow:

The SMB stops before hitting the pillar. In the terminal, the service was called to stop the robot.

Post collision

Run the launch file with the following command:

roslaunch smb_highlevel_controller smb_highlevel_controller.launch start_robot:="true" auto_emergency:="true" prior_collision:="false"

By default, the robot will stop after hitting the pillar based on collision_threshold which is the maximum change of IMU on x axis before the service is called that is specified in the default_parameters.yaml.

To get a proper collision_threshold, rqt_multiplot is launched with the xy_imu_multiplot.xml config.

During the collision, the x-axis IMU plot can be seen as follow:

The plot of IMU x-axis and y-axis plot during collision, there is a sudden spike of IMU x-axis with the value change of around 1.8

The overall output should be as follow:

The SMB stops after hitting the pillar. In the terminal, the service was called to stop the robot.

Files

SmbHighlevelController.py

• Add parameters for the service (i.e. service name, start_robot).
• Add start/stop server, service name and service callback based on the service call with SetBool.
• Add bool isStart_ to move the robot only if it is enabled.

stop_condition

• As the client, call the service to stop SMB robot with SetBool based on the parameter prior_collision.
• The parameters that can be adjusted are max_distance_to_pillar for prior collision and collision_threshold for post collision.
• This node is called from the launch file if the parameter auto_emergency is enabled.

xy_imu_multiplot.xml:

• Create an x/y-plane plot of the smb and the x and y IMU data over time.

smb_highlevel_controller.launch:

• Use arguments to load some of the params instead of rosparam from default_parameters.yaml. This allow arguments to be passed from command line.
• Add stop_condition_node to automatically stop the SMB prior/post collision if enabled.
• Change rqt_multiplot config with xy_imu_multiplot.xml to plot the path of smb in x/y plane as well as x and y IMU data over time.

CMakeLists.txt

• Add std_srvs in find_package
• Install python executable for stop_condition node with catkin_install_python .

package.xml

• Add depend std_srvs to use SetBool service.