YhErAyfQ

1Skip to content
2Search or jump to…
3
4Pull requests
5Issues
6Marketplace
7Explore
8 
9@hesterliu 
10Learn Git and GitHub without any code!
11Using the Hello World guide, you’ll start a branch, write comments, and open a pull request.
12
13
144
15011UNSW-MTRN2500/assignment2-release
16 Code Issues 0 Pull requests 0 Actions Projects 0 Wiki Security Insights
17assignment2-release/assignment2_spec.md
18@vong0 vong0 Added link to find axes mappings
198028c9a 7 days ago
20@ljden@zhi-zhang@vong0
21392 lines (291 sloc)  25.2 KB
22  
23The signature of the class you will need to implemented in this assignment is provided in .hpp file in the include directory. Write your code in the corresponding .cpp file. You can add any extra code to student_helper.hpp and student_helper.cpp
24
25MTRN2500 Assignment 2 Specification
26Purpose
27The purpose of this assignment is to familiarise yourself with larger programming projects. In this assignment, you will be applying your newly learned C++ skills to allow for controller input to be consumed and processed by industry standard middleware (Robot Operating System - ROS) and visualised.
28
29Assignment Instructions
30You will be submitting your code to the private Github assignment2 repository created for you.
31
32After the repository was created, you must do the following:
33
34Add an empty file to the root directory with your zid as the filename in the format z0000000.txt.
35Create a review branch. Style task will be assessed on that branch.
36Create a development branch. You should do work in a development branch and only merge working code into the master branch.
37Note: failing to do the above will result you getting 0 marks for the assignment.
38
39mtrn2500_ws in the provided MTRN2500 virtual machine is the recommended location for you to clone the git repository.
40
41Demonstration will be during your tutorial in week 9 (note this is one week later than advised in the course outline). Students in the Monday tutorial will demonstrate in week 10 due to the public holiday.
42
43Code style submission is due at 5pm Friday week 9 (15 November 2019).
44
45For the style task, create a pull request of your commit from the master branch to review branch. Your demonstrator will do a code review on your pull request. Your demonstrator can give early feedback if you create the pull request and ask your demonstrator to review it in your lab session.
46
47Final assessment of the style task will be be based on the pull request to review from the last commit to the master branch before the code style submission due date.
48
49Useful Information
50this
51There is a special pointer called this that is provided to all methods that are run from instantiated objects. It is a pointer to the instantiated object that the method is being run on. It is, however, not necessary to use when referring to member variables and methods as you are able to refer to member variables and member methods by their name without using the keyword this.
52
53Templates
54In addition to regular parameters, some classes and function may take types as parameter. For example the standard template library container can be adapted to work with different types by taking in a type parameter. You will learn more about this later, at this stage you just need to know how to use them. The type parameter is passed in < > after name and before ( ) as in std::vector <int> ().
55
56Lambda (anonymous) functions
57Lambda functions are special functions that can be declared in C++. These functions are different to functions that you are familiar with when using regular standalone functions or methods on a class. They functions are declared within the scope of some other function and exist as a function pointer within the scope that they're declared in, which means they also abide by the scope that they are defined in (see the Week 6 Tutorial for more information about this). Unlike regular classes and functions which are generated once (during compilation of the program) and have a fixed name for the entire runtime of the program, lambda functions are dynamic in the sense that their function code (the code that is executed when the function is called) is generated at runtime but their names are not, as well as being not being a reusable function and generally representing functions which have no purpose being named. Additionally, they are generally short functions that should generally not be longer than five lines - at this point, you would benefit more from declaring a regular function.
58
59An example of how to declare a lambda function can be found below:
60
61#include <algorithm>
62#include <iostream>
63#include <vector>
64
65int main (void) {
66  std::vector<double> numbers{1, 2, 3, 8, 4};
67
68  // store the largest number in the maxValue variable
69  double maxValue = 0;
70  std::for_each(numbers.begin(), numbers.end(), [&maxValue](const double& value) {
71    if (value > maxValue) {
72      maxValue = value;
73    }
74  });
75
76  std::cout << "Largest value is: " << maxValue << std::endl;
77  return 0;
78}
79Let's work through this one section at a time.
80
81#include <algorithm>
82#include <iostream>
83#include <vector>
84Including the necessary libraries for our std::for_each function, outputting to the terminal and for storing our values.
85
86  std::vector<double> numbers{1, 2, 3, 8, 4};
87Creating a vector of numbers using an initialiser list (which is a special kind of constructor).
88
89  double maxValue = 0;
90Creating a variable where we will be storing the largest value in the vector.
91
92  std::for_each(numbers.begin(), numbers.end(), [&maxValue](const double& value) {
93    if (value > maxValue) {
94      maxValue = value;
95    }
96  });
97Here we have the main portion of this example - defining the lambda function. We first have the use of a std::for_each function, which takes in the starting and ending vector iterators which define where we want this lambda function to operate on. We then have a lambda function which is defined that is comprised of three sections:
98
99[&maxValue]: variables which are captured
100(const double& value): the input arguments to this function, where the chosen algorithm (which is std::for_each in this case) passes each value between the start and end iterators
101{ ... }: the contents of the function
102The parameters which are captured (the first section [ ... ]) declare which variables that exist in the scope that this lambda function is defined in which are to be passed to the lambda function. If the & was omitted, the parameter would be copied and the copy is passed to the lambda function. With the &, a reference to the original parameter is created and the reference is passed in to the lambda function. The lambda function can also store the this pointer by value using the syntex [this]
103
104  std::cout << "Largest value is: " << maxValue << std::endl;
105Outputting the value of the largest number in the previously created vector to the terminal in a nicely formatted string.
106
107Introduction to Robot Operating System (ROS)
108An introduction to ROS and the purpose behind it can be found in the Week 5 Tutorial. There are a few concepts that are integral to how ROS works.
109
110Packages
111ROS organises all user-written software in a specific way. All libraries and executables are organised into separate packages which each represent a specific area of functionality. For example: given a bipedal humanoid robot, one would organise a package that deals purely with moving the limbs of the robot. Another would deal purely with camera input. Another would deal with high-level movement planning logic, etc.
112
113Nodes
114Given that packages are libraries and executables that are represent specific areas of functionality, nodes can be understood as specific tasks that are undergone to perform some processing, communication or visualisation. The nodes are the final executables which are orchestrated by ROS so that they can communicate between each other. There can be multiple ROS nodes in a single ROS package.
115
116To create a node, you must first create a class which you wish to use to conceptually represent the node. Consider the example where you wish to write a node called ColourImageProcessing_node through the ColorImageProcessing class which performs colour image processing in a camera processing package. You can specify that you wish to have that class be a node in the following way:
117
118class ColorImageProcessing : public rclcpp::Node {
119  ColorImageProcessing() : rclcpp::Node{std::string{"ColorImageProcessing_node"}} {}
120}
121Topics
122Topics are the main method of communicating between ROS packages. You can think of topics as channels of communication between nodes. ROS nodes can not only be run on a single machine (as you will be facilitating in this assignment), but they can also be run on multiple machines in a distributed fashion. ROS handles the networking and communication portion of this through these topics. They work using a Publisher / Subscriber model. This means that any node which subscribes to a particular topic will receive a message sent by a node which publishes to the same topic.
123
124FUN FACT: Nodes can both publish and subscribe to multiple topics all at the same time.
125
126Subscribers
127In order for a node to receive information from another node, they must first subscribe to a topic. There are three pieces of information required to allow a node to subscribe to a topic. These are:
128
129topic name (string naming the topic being subscribed to)
130queue length (unsigned integer representing the amount of messages that can be stored in the message cue before the node refuses to accept any further messages through this subscription)
131callback (a function pointer for a function that is run when a new message is received on the subscribed topic)
132Here is an example of how to create one:
133
134class Example : public rclcpp::Node {
135  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr example_subscriber_;
136  
137  Example() : rclcpp::Node{std::string{"Example_node"}} {
138    this->example_subscriber_ = create_subscription<sensor_msgs::msg::Joy>(
139                                std::string("topic"),
140                                10,
141                                [this](sensor_msgs::msg::Joy::UniquePtr joy_message){
142                                  // callback implementation
143                                }
144                              );
145    }
146  }
147}
148Or, if you would prefer to implement your callback in a separate function without using a lambda function:
149
150class Example : public rclcpp::Node {
151  rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr example_subscriber_;
152
153  Example() : rclcpp::Node{std::string{"Example_node"}} {
154    auto callback = std::bind(&Example::topic_callback, this, std::placeholders::_1);
155    this->example_subscriber_ = create_subscription<sensor_msgs::msg::Joy>(std::string("topic"), 10, callback);
156  }
157
158  void topic_callback(sensor_msgs::msg::Joy::UniquePtr joy_message) {
159    // callback implementation
160  }
161}
162Note that when we attach the callback function we need to pass in the this argument, however we cannot do that explicitly and therefore we must bind a partial function to the callback with std::bind to pass in the hidden this argument. We add the placeholder variable std::placeholders::_1 to the callback to allow the message pointer to be passed into our callback function.
163
164Publishers
165In order for information to be communicated to other components of the system, it must first be packaged and sent. This is the role of a publisher. In order to publish a message, you must first create a data structure which defines this message, populate it with the required information and then publish it. An example can be found below.
166
167class Example : public rclcpp::Node {
168  rclcpp::Publisher<geometry_msgs::msg::String> string_publisher_;
169
170  Example() : rclcpp::Node{std::string{"node_name"}} {
171    this->string_publisher_ = create_publisher<geometry_msgs::msg::String>(std::string{"ExampleTopic"}, 10)
172  }
173};
174
175// within some other function / method
176auto string_information = sensor_msgs::msg::String();
177// the fields of a particular structure can be found in the "msgs" directory of a "msgs" package if it is a custom message
178string_information.message = std::string("This is some example text that will be sent through a topic");
179string_publisher_->publish(stringInformation);
180Timers
181Another method capable of automatically causing some events to be kicked off at a regular time interval is through the use of "wall timers", which allow you to set a callback function to be run continually at some time interval. The time requires two parameters - the time interval that the callback will be run in, and a function pointer to the callback itself. An example of how to set a wall timer can be found below:
182
183#include <chrono>
184
185class Example : public rclcpp::Node {
186  rclcpp::TimerBase::SharedPtr timer_;
187
188  Example() : rclcpp::Node{std::string{"Example_node"}} {
189    this->timer_ = create_wall_timer(std::chrono::milliseconds{100},
190                    [this]() {
191                        // function which implements the regularly occurring task
192                      }
193                    );
194  }
195};
196Task 0: Launch joystick driver (no marks)
197NOTE: All the messages you need to send and receive will be in the ROS message namespace /zxxxxxxx/where zxxxxxxx will be provided to you on the day of demonstration. For the rest of this document we will use /z0000000 as a place holder
198
199joy_node in the package joy is an executable that will interface with an Xbox controller and send inputs as sensor_msgs::msg::Joy message to the topic /joy. A launch file is provided to redirect the message to /z0000000/joy, the launch file will also configure joy_node to read from the correct device.
200
201To launch the joystick driver, in the directory you cloned the git repository:
202
203Install the launch script using the command: . mtrn2500_make
204Launch joy_node using the command: ros2 launch assignment2 joy_node.py
205A quick way to show message being sent to a topic is use the command ros2 topic echo /z0000000/joy. You can check the list of current topic by using the command ros2 topic list.
206You can change the topic the joy message is redirected to by editing joy_node.py and joy_node_config.yaml file in the launch folder.
207
208Task 1: Read the joystick (6 marks)
209This task is handled by the class JoystickListener in the header joystick_listener.hpp.
210
211The purpose of this task is to decode the sensor_msgs::msg::Joy message from the topic zxxxxxxx/joy to get the player actions. Send the player actions as a geometry_msgs::msg::AccelStamped message to the topic /z0000000/acceleration. i.e. the JoystickListener class will convert joystick inputs into acceleration values and publish these to be used by any subsequent nodes.
212
213JoystickListener class contains:
214
215joystick_input_ is a std::shared_ptr to a subscriber used to listen to /z0000000/joy.
216acceleration_output_ is a std::shared_ptr to a publisher used to send the acceleration data to /z0000000/acceleration.
217zid_ is a string that contains z0000000.
218config_ is a joystick_config struct hold your joystick configuration data.
219joy_message_callback is a method that will be called every time a new message is received by joystick_input_, the new message will be passed in as joy_message parameter. You need to make sure to register this function when you create joystick_input_.
220JoystickConfig struct is defined in config_parser.hpp as:
221
222struct JoystickConfig
223{
224public:
225    std::size_t speed_plus_axis;
226    std::size_t speed_minus_axis;
227    std::size_t steering_axis;
228    double steering_deadzone;
229    double speed_deadzone;
230};
231sensor_msgs::msg::Joy contains
232
233header
234string frame_id: std::string containing the transform frame with which this data is associated.
235builtin_interfaces/Time stamp: Two-integer timestamp that is expressed as seconds and nanoseconds.
236float32[] axes: std::vector<float> containing the axes measurements from a joystick.
237int32[] buttons std::vector<int32_t> containing the buttons measurements from a joystick.
238All the other ROS2 message has similar structure. Documentation can be found at ROS2 common interface
239
240Subtask A : Print all axis and buttons status to std::cout (2 marks)
241In the method joy_message_callback:
242
243use std::for_each to iterate through the axes vector and print out the value of each axes in a single line. Separate each value with a tab. Format: 0.0\t1.234567\t1.0\t2.0\t...\t\n.
244Use std::count or std::count_if to find out how many buttons have been pressed simultaneously. Print the value to std::cout in the format: Total number of buttons pressed is {}.\n, replace {} with total number of buttons pressed.
245Sub-task B: Calculate linear and angular acceleration inputs (4 marks)
246The axes we will need to read from are defined in config_ .
247
248speed_plus_axis: Positive linear acceleration axis
249speed_minus_axis: Negative linear acceleration axis
250steering_axis: Angular acceleration axis
251An axis could be a trigger or a joystick on the Xbox controller. For example, if we were using a wired Xbox 360 controller:
252
253speed_plus_axis = 0: Read from the Left/Right left joystick
254speed_minus_axis = 1: Read from the Up/Down left joystick
255steering_axis = 2: Read from the left trigger
256The axes mappings for each controller can be found at ROS Joy Documentation
257
258In the method joy_message_callback:
259
260Calculate positive linear acceleration. Axis input in the range of [-1.0, 1.0] corresponds to an acceleration output in the range of [0.0, 1.0].
261Calculate negative linear acceleration. Axis input in the range of [-1.0, 1.0] corresponds to an acceleration output in the range of [0.0, -1.0].
262Calculate the angular acceleration. Axis input in the range of [-1.0, 1.0] corresponds to an acceleration output in the range of [-1.0, 1.0].
263Calculate the net linear acceleration equal to the sum of positive and negative linear acceleration scaled to the range of [-1.0, 1.0].
264Due to the physical design the joystick value may be a small non-zero value at the neutral position. This is known as deadzone. We want to treat those inputs as zero. Treat input within plus or minus deadzone value as zero. Scale the input such that input [deadzone , 1.0] scales to [0.0 , 1.0] and [-deadzone , -1.0] to [0.0, and -1.0], The deadzone value is specified in config_.speed_deadzone and config_.steering_deadzone.
265Send a geometry_msgs::msg::AccelStamped using publish method of the publisher pointed byacceleration_output_.
266The geometry_msgs::msg::AccelStamped message should contain:
267
268Net acceleration as the x component of the linear part of accel member in the message.
269Angular acceleration as the z component of the angular part of accel member in the message.
270Use the time stamp of the joystick message as the time stamp in the header
271Use zid as the header.frame_id.
272You can check the message by echoing it to the terminal by using the command:
273
274ros2 topic list
275ros2 topic echo /z0000000/joy
276TASK 2: Velocity and Pose (4.5 marks)
277Calculate the velocity and pose of the vehicle at regular interval using the acceleration input. Velocity is calculated with respect to the vehicle body and pose with respect to the global coordinate frame.
278
279Sub-task A: Calculate linear and angular velocity (1.5 marks)
280This task is handled by the class VelocityKinematic in the header velocity_kinematic.hpp. This class will subscribe to the topic /z0000000/acceleration from the previous task and integrate the acceleration to obtain velocity. Periodically send the calculated velocity as a geometry_msgs::msg::TwistStamped message to the topic /z0000000/velocity.
281
282Assume initial velocities are both zero.
283Calculate time difference dt between the last time stamp and the current time. You can get the current time by calling the function now(). Hint: The ROS time class has a method called seconds();
284Integrate the velocity at the rate refresh_period milliseconds. refresh_period is a constructor parameter.
285You can use the formula: v(t+dt) = v(t) + dt * a(t)
286Print the accelerations and velocities to the screen: Format: DT: {}, Linear Acceleration: {}, Linear velocity: {}, Angular acceleration: {}, Angular velocity: {}. {} indicates the position of each value and should not be printed.
287Send a geometry_msgs::msg::TwistStamped to the topic /z0000000/velocity at frequency of 1/refresh_period.
288The geometry_msgs::msg::TwistStamped message should contain:
289
290Linear velocity as the x component of the linear part of twist member in message.
291Angular velocity as the z component of the angular part of twist member in message.
292Set the time used to calculate dt as the time stamp in the header
293Use zid as the header.frame_id.
294Sub-task B: Error handling (1.5 marks)
295Scale the linear and angular acceleration based on max_linear_acceleration and max_angular_acceleration set in the kinematic_limits config_.
296Limit maximum velocity to based on the plus/minus maximum velocity set in the kinematic_limits config_.
297Do nothing until the first geometry_msgs::msg::AccelStamped has been received.
298If the last velocity message received was older then 10 seconds, consider the communication lost and set the acceleration and velocity to zero. Print "Communication lost.\n" and stop sending geometry_msgs::msg::TwistStamped until a new geometry_msgs::msg::AccelStamped has been received.
299Sub-task C: Calculate position and orientation (pose) (1.5 marks)
300This task is handled by the class PoseKinematic in the header pose_kinematic.hpp. This class will subscribe to the topic /z0000000/velocity from the last part and integrate the velocity to get current pose. Periodically send the calculated pose as a geometry_msgs::msg::PoseStamped message to the topic /z0000000/pose.
301
302Assume starting at the origin (0.0, 0.0) with heading = 0.
303Calculate time difference between the last time stamp and the current time.
304Convert the velocity to the global coordinate frame using trigonometric functions.
305Calculate the position and orientation at the rate refresh_period milliseconds. refresh_period is a constructor parameter.
306The geometry_msgs::msg::PoseStamped message should contain"
307
308(x,y) coordinate as the position part of pose member in message.
309Angular acceleration as the z component of the orientation part of pose member in message.
310Use the time used to calculate dt as the time stamp in the header
311Use zid as the header.frame_id.
312Task 3: Parse config file (4.5 marks)
313This task is handled by the class ConfigReader and ConfigParser in the header config_parser.hpp.
314
315Read and parse the config file as opposed to using hard-coded configuration parameters. config_reader will read in the config file into a std::unordered_map<std::string,std::string>, while config_parse will convert the result from config_reader to actual configurations.
316
317Sub-task A: Read the config file (1.5 marks)
318This task is handled by the class ConfigReader in the header config_parser.hpp.
319
320Location of the config file will be passed in as a commandline argument.
321Modify the main program to read from a config file instead of std::cin.
322Read each line config file in the config_reader constructor. Print each line to the terminal in the format: Line {number}: {string}\n. {} indicate the position of each value and should not be printed.
323Finish reading the file when an empty line was encounter.
324Sub-task B: Read the config file (1.5 marks)
325This task is handled by the class ConfigReader in the header config_parser.hpp.
326
327The config file will be in the format key : value. All the keys will be unique. Key and value may be surrounded by arbitrary amount of spaces. Each line consist of only a single pair of key and value.
328
329Split each line into key and value string.
330Trim leading and trailing space from key and value.
331Store the config in config_ as pair of key and value strings.
332After all the lines have been read, iterate through config_ and print key and value pairs. Format key: \"{key}\", value: \"{value}\"\n. {} indicates the position of the value and should be not printed.
333Sub-task C: Parse the config file (1.5 marks)
334This task is handled by the class ConfigParser in the header config_parser.hpp.
335
336config_parser constructor should read from config and initialise all the members.
337    std::string const zid_;
338    std::chrono::milliseconds const refresh_period_;
339    joystick_config const joy_config_;
340    kinematic_limits const kinematic_config_;
341Also implement the getter methods.
342Style and Source Control (5 mark)
343The most important thing with style is to make your code understandable to other people.
344
345Follow the course style guide which is based on ROS2 development guide.
346Neat and tidy code, style is consistent throughout.
347Good choice of names that are meaningful.
348Good use of functions to split code into manageable segments and avoid code duplication.
349Good use of c++ features.
350Good documentation and comments.
351No error or warning message when compiling.
352master branch must be able to compile and work.
353Consistent use of source management. Create development branches. Keep your Github repository up to date with you work. Write meaningful commit message.
354Finally
355For full marks on each component you will need to demonstrate it working and be able to explain the code you have implemented to your demonstrator within your demonstration.
356
357For assistance, please post questions in the course MS Teams Assignment2 channel.
358
359Enjoy getting started in C++, as the following assignment will build on much of this content.
360
361© 2019 GitHub, Inc.
362Terms
363Privacy
364Security
365Status
366Help
367Contact GitHub
368Pricing
369API
370Training
371Blog
372About