NelsenEW / eth-zurich-solution

ETH Zurich ROS Exercise solution

ETH Zürich ROS Solution

This is an unofficial solution for eth zurich ros 2021 course for MLDA Robotics. The solution has been well tested for both ROS Melodic and Noetic.

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.