Implement lane change maneuver

[Goldob]

Implement node change_lane in package selfie_control, which will be responsible for publishing the setpoint for PID controller (#30) depending on which lane the vehicle is supposed to drive at given moment. The setpoint itself is a linear offset (in meters) from the center line. For now a fixed lane width is assumed. By default the vehicle should drive on the right lane.

The node should implement a ChangeLane ActionServer named change_lane. The action handling flow should look like follows:

1. New goal is received
2. If target lane is the same as the one we're currently on, the goal is rejected and we do not proceed further.
3. Otherwise, the goal is accepted, setpoint is changed accordingly and the correct turn indicator is turned on. We keep track of the vehicle's position on the road.
4. Once the vehicle reaches the center of the target lane (within marigin of error), the goal is marked as succeeded and the turn indicator is turned off.

Refer to actionlib package documentation to learn more about actions, especially the section on goal state transitions.

change_lane

Parameters

~lane_width (float)
Assumed lane width (in meters).

~error_margin (float)
Maximum distance from center of the target lane (in meters) until the lane change maneuver is considered complete.

Subscribed topics

position_offset (std_msgs/Float64)
Current distance from the center line (in meters). Positive offset is to the left.

Published topics

target_offset (std_msgs/Float64)
Desirable distance from the center line (in meters), used as setpoint for PID controller. Positive offset is to the left. The value should be set depending on which lane the vehicle is supposed to drive on at given moment.

left_turn_indicator (std_msgs/Bool)
right_turn_indicator (std_msgs/Bool)
Flags used to turn on left and right turn indicator, respectively. Should be set to true for the whole period during which the indicator should be turned on - blinking will be handled in a separate program.

[Goldob]

This issue depends on #34.

[nels3]

Status: in progress

[nels3]

@Goldob
Please look at test node. I have implemented the demo version of code. I need to know if this seems ok.
Please do:
A) branch change_line
B) roscore

1. rosrun selfie_control demo_position (one command window)
2. rosrun selfie_control change_lane _lane_width:=1 _error_margin:=0.1 (another command window, width positive, error_margin positive)
3. rosrun selfie_control demo_change_lane _lane:=l (or _lane:=r, another command window, should run commands with l or r argument)

If this is what was needed we could talk about what should be pushed to develop branch (I don't know about demo nodes and I will delete ROS_INFO also inside it couse this is only for demo).

[Goldob]

@nels3 I looked, and I have some comments. Please create a PR where we can discuss them.

[Goldob]

Implemented in #37.