try

.gitignore

@@ -1,3 +1,4 @@
+tmp
 *~
 *.bag
 *.pyc

.mybashrc

@@ -0,0 +1 @@
+export FOO="BAR"

2environment.sh

@@ -0,0 +1,39 @@
+#!/bin/bash
+# New policy: DUCKIETOWN_ROOT is implicit in the choice of running this script
+
+[ -z "$HOSTNAME"        ] && { echo -e "\n\nThe variable HOSTNAME is not set. I need this info for setting up ROS. \n\n\n\n"; return 2;       }
+
+# Do not compile Lisp messages
+# XXX: not sure if this is the place to put this.
+export ROS_LANG_DISABLE=gennodejs:geneus:genlisp
+
+shell=`basename $SHELL`
+echo "Activating ROS with shell: $SHELL"
+source /opt/ros/kinetic/setup.$shell
+
+export HOSTNAME=$HOSTNAME
+export ROS_HOSTNAME=$HOSTNAME.autolab.ethz.ch
+echo "Set ROS_HOSTNAME to: $ROS_HOSTNAME"
+
+export DUCKIETOWN_ROOT=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )
+echo "Set DUCKIETOWN_ROOT to: $DUCKIETOWN_ROOT"
+
+export PYTHONPATH=$DUCKIETOWN_ROOT/catkin_ws/src:$PYTHONPATH
+echo "Set PYTHONPATH to: $PYTHONPATH"
+
+# Cannot make machines before building
+# echo "Building machines file..."
+# make -C $DUCKIETOWN_ROOT machines
+
+echo "Activating development environment..."
+source $DUCKIETOWN_ROOT/catkin_ws/devel/setup.$shell
+
+if [ 2015 -ge $(date +%Y) ];
+then
+    >&2 echo "Error! Time travel detected. System time is: $(date)"
+fi
+
+# add the shortcuts to the path
+export PATH=$PATH:$DUCKIETOWN_ROOT/shortcuts
+
+exec "$@" #Passes arguments. Need this for ROS remote launching to work.

2set_ros_master.sh

@@ -0,0 +1,11 @@
+#!/usr/bin/env bash
+echo "Setting ROS_MASTER_URI..."
+if [ $# -gt 0 ]; then
+	# provided a hostname, use it as ROS_MASTER_URI
+	export ROS_MASTER_URI=http://$1.autolab.ethz.ch:11311/
+else
+	[ -z "$HOSTNAME"        ] && { echo -e "\n\nThe variable HOSTNAME is not set. I need this info for setting up ROS. \n\n\n\n"; return 2;       }
+	echo "No hostname provided. Using $HOSTNAME."
+	export ROS_MASTER_URI=http://$HOSTNAME.autolab.ethz.ch:11311/
+fi
+echo "ROS_MASTER_URI set to $ROS_MASTER_URI"

Makefile

@@ -28,3 +28,5 @@ include Makefiles/Makefile.hw_test.mk
 include Makefiles/Makefile.maintenance.mk
 include Makefiles/Makefile.openhouse.mk
 include Makefiles/Makefile.log.mk
+include Makefiles/Makefile.demos2017.mk
+include Makefiles/Makefile.megacity.mk

Makefiles/Makefile.demos.mk

@@ -5,7 +5,11 @@ demos:
 	@echo
 	@echo TODO: to write
 	@echo
-
+
+### Intersection navigation demo
+demo-intersection-navigation: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch intersection_navigation intersection_navigation_node.launch"
+
 ### These are not using master.launch
 demo-joystick: check-environment
 	bash -c "source environment.sh; source set_ros_master.sh;  roslaunch duckietown joystick.launch veh:=$(vehicle_name)"
@@ -22,7 +26,6 @@ demo-joystick-camera-high-speed: check-environment
 demo-line_detector: check-environment
 	bash -c "source environment.sh; source set_ros_master.sh; roslaunch duckietown line_detector.launch veh:=$(vehicle_name)"
 
-
 demo-joystick-perception: check-environment
 	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos master.launch fsm_file_name:=joystick"
 
@@ -38,6 +41,8 @@ demo-led-fancy2: check-environment
 demo-led-blink-%: check-environment
 	bash -c "source environment.sh; rosrun rgb_led blink $*"
 
+demo-implicit_coordination: check-environment
+	bash -c "source environment.sh; source set_vehicle_name.sh; roslaunch duckietown_demos implicit_coordination.launch"
 demo-line_detector-default:     demo-line_detector-quiet-default
 demo-line_detector-guy:         demo-line_detector-quiet-guy
 demo-line_detector-universal:   demo-line_detector-quiet-universal
@@ -46,9 +51,15 @@ demo-line_detector-default_ld2: demo-line_detector-quiet-default_ld2
 demo-line_detector-quiet-%: check-environment
 	bash -c "source environment.sh; source set_ros_master.sh; roslaunch duckietown line_detector.launch veh:=$(vehicle_name) line_detector_param_file_name:=$* verbose:=false"
 
+demo-vehicle_follow_leader: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos vehicle_avoid.launch"
 
 
 # Basic demos
 # traffic lights
 traffic-light: check-environment
 	bash -c "source environment.sh; source set_ros_master.sh; roslaunch traffic_light traffic_light_node.launch veh:=$(vehicle_name)"
+
+
+turn-duration-%: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh $*; source set_vehicle_name.sh $*; rosparam set /$*/open_loop_intersection_control_node/turn_$(type) '[[0.8, 0.43, 0],[$(time), 0.43, 2.896],[0.8, 0.43, 0.0]]'; "

Makefiles/Makefile.demos2017.mk

@@ -0,0 +1,14 @@
+
+demos2017:
+	@echo "$(sep) 2017 demos"
+	@echo
+	@echo "These were the 2017 demos."
+	@echo
+	@echo TODO: to write
+	@echo
+
+coordination2017: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos stop_sign_coordination2017.launch"
+
+trafficLights2017: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos traffic_light_coordination.launch"

Makefiles/Makefile.megacity.mk

@@ -0,0 +1,43 @@
+megacity: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos megacity.launch"
+
+megacity-%: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos megacity.launch city:=$*"
+
+megacity-no-maintenance: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos megacity-no-maintenance.launch"
+
+tcp-server: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos tcp_server.launch"
+
+virjoy-%: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh $*; source set_vehicle_name.sh $*; python misc/virtualJoy/virtualJoy.py"
+
+parallel-autonomy: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos parallel_autonomy.launch"
+
+formula-d-wheel-%: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh $*; source set_vehicle_name.sh $*; roslaunch duckietown_demos formula_D_steering_wheel.launch veh:=$*"
+
+formula-d: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; roslaunch duckietown_demos formula_D.launch"
+
+camera-%: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh $*; source set_vehicle_name.sh $*; rosrun image_view image_view image:='/$*/camera_node/image' _image_transport:=compressed"
+camera-joy-%: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh $*; source set_vehicle_name.sh $*; source misc/camera_joy.sh $*"
+
+set-in-charger-%: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh $*; source set_vehicle_name.sh $*; rostopic pub -1 /$*/maintenance_control_node/go_charging std_msgs/Bool True; rostopic pub -1 /$*/maintenance_control_node/set_state std_msgs/String 'CHARGING'; rosparam set /$*/charging_control_node/charger 4"
+
+prepare: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh; source set_vehicle_name.sh; cd $(DUCKIEFLEET_ROOT); git pull; cd $(DUCKIETOWN_ROOT)"
+
+prepare-%: check-environment
+	bash -c "source environment.sh; source set_ros_master.sh $*; source set_vehicle_name.sh $*"
+
+killall: check-environment
+	bash -c "sudo pkill -9 python"
+
+git-reset: check-environment
+	bash -c "git reset --hard HEAD"

README.md

@@ -16,7 +16,7 @@ This is the main software repository for [the Duckietown project][main]. It cont
 * `master19`: will start in 2019
 
 #### Continuous integration
- 
+
 See [the continuous integration console](https://circleci.com/gh/duckietown/Software/).
 
 Master is now a protected branch. [See docs here](https://github.com/blog/2051-protected-branches-and-required-status-checks).

catkin_ws/src/00-infrastructure/duckietown/config/baseline/calibration/camera_extrinsic/lex.yaml

@@ -0,0 +1 @@
+homography: [-4.89775e-05, -0.0002150858, -0.1818273, 0.00099274, 1.202336e-06, -0.3280241, -0.0004281805, -0.007185673, 1]

catkin_ws/src/00-infrastructure/duckietown/config/baseline/calibration/kinematics/lex.yaml

@@ -0,0 +1,7 @@
+baseline: 0.1
+calibration_time: 2017-10-13-13-27-20
+gain: 1.100000023841858
+k: 27.0
+limit: 1.0
+radius: 0.0318
+trim: -0.125

catkin_ws/src/00-infrastructure/duckietown/config/baseline/calibration/lex.yaml

@@ -0,0 +1,10 @@
+homography:
+- 9.266699812353547e-05
+- -0.00034062999909511866
+- -0.29183023077323283
+- 0.0012271255814808567
+- 7.840958341099328e-06
+- -0.3899202973949822
+- 0.0004474816741134259
+- -0.009284176659120184
+- 0.9999999999999999

catkin_ws/src/00-infrastructure/duckietown/config/baseline/charging_control/charging_control_node/default.yaml

@@ -0,0 +1,3 @@
+drive_time: 120
+charge_time: 120
+do_calib: False

catkin_ws/src/00-infrastructure/duckietown/config/baseline/dagu_car/car_cmd_switch_node/default.yaml

@@ -1,29 +1,47 @@
 mode_topic: "fsm_node/mode"
 source_topics: #Mapping from cmd source name to topics
   lane: "lane_controller_node/car_cmd"
-  intersection: "open_loop_intersection_control_node/car_cmd"
+
+  # intersection: "open_loop_intersection_control_node/car_cmd"
   # coordination: "simple_stop_controller_node/car_cmd"
+
+  # intersection: "intersection_navigation_node/cmds_out" #NOTE for open loop intersection navigation
+  intersection: "lane_controller_node/car_cmd" #NOTE for closed loop intersection navigation
+  coordination: "coordinator_node/car_cmd"
+  #coordination: "implicit_coordination_node/car_cmd" #NOTE for implicit coordination
+
   joystick: "joy_mapper_node/car_cmd"
   vehicle:  "vehicle_avoidance_control_node/car_cmd"
   obstacle: "simple_stop_controller_node/car_cmd"
   parallel_autonomy: "lane_supervisor_node/car_cmd"
   recovery: "lane_recovery_node/car_cmd" # not implemented yet
   deep_learning: "duckiebot_il_lane_following/car_cmd"
   parking: "lane_controller_node/car_cmd"
+  calib: "auto_calibration_node/car_cmd"
+  calib_calc: "auto_calibration_calculation_node/car_cmd"
   stop: "simple_stop_controller_node/car_cmd"
+  #stop: "stop_node/car_cmd" if above does not work this one works
+
 mappings: #Mapping from FSMStates.state to cmd source names. Allows different FSM mode to use the same source.
-  LANE_FOLLOWING: "lane"
+  LANE_FOLLOWING: "vehicle" #TODO blackmagic
   LANE_FOLLOWING_AVOID: "lane"
+  ARRIVE_AT_STOP_LINE: "stop"
   INTERSECTION_CONTROL: "intersection"
   INTERSECTION_COORDINATION: "stop"
   NORMAL_JOYSTICK_CONTROL: "joystick"
-  SAFE_JOYSTICK_CONTROL: "joystick"
+  SAFE_JOYSTICK_CONTROL: "parallel_autonomy"
   AVOID_OBSTACLE: "obstacle"
   AVOID_VEHICLE:    "vehicle"
   LANE_RECOVERY:     "recovery"
   INTERSECTION_RECOVERY: "recovery"
-  PARALLEL_AUTONOMY: "parallel_autonomy"  
+  PARALLEL_AUTONOMY: "parallel_autonomy"
   LOCALIZATION: "stop"
   DEEP_LANE_FOLLOWING: "deep_learning"
   PARKING: "parking"
   ARRIVE_AT_STOP_LINE: "stop"
+  INTERSECTION_PLANNING: "stop"
+  CALIBRATING: "calib"
+  CALIBRATING_CALC: "calib_calc"
+
+  IN_CHARGING_AREA: "vehicle"
+  CHARGING_FIRST_IN_LINE: "stop"

catkin_ws/src/00-infrastructure/duckietown/config/baseline/dagu_car/car_cmd_switch_node/default2017.yaml

@@ -0,0 +1,22 @@
+mode_topic: "fsm_node/mode"
+source_topics: #Mapping from cmd source name to topics
+  lane: "lane_controller_node/car_cmd"
+  intersection: "open_loop_intersection_control_node/car_cmd"
+  coordination: "coordinator_node/car_cmd"
+  joystick: "joy_mapper_node/car_cmd"
+  vehicle:  "vehicle_avoidance_control_node/car_cmd"
+  obstacle: "simple_stop_controller_node/car_cmd"
+  parallel_autonomy: "lane_supervisor_node/car_cmd"
+  recovery: "lane_recovery_node/car_cmd" # not implemented yet
+mappings: #Mapping from FSMStates.state to cmd source names. Allows different FSM mode to use the same source.
+  LANE_FOLLOWING: "lane"
+  LANE_FOLLOWING_AVOID: "lane"
+  INTERSECTION_CONTROL: "intersection"
+  COORDINATION: "coordination"
+  JOYSTICK_CONTROL: "joystick"
+  AVOID_OBSTACLE: "obstacle"
+  AVOID_VEHICLE:    "vehicle"
+  LANE_RECOVERY:     "recovery"
+  INTERSECTION_RECOVERY: "recovery"
+  PARALLEL_AUTONOMY: "parallel_autonomy"  
+  LOCALIZATION: "stop"

catkin_ws/src/00-infrastructure/duckietown/config/baseline/dagu_car/car_cmd_switch_node/no_veh_avoid.yaml

@@ -0,0 +1,29 @@
+mode_topic: "fsm_node/mode"
+source_topics: #Mapping from cmd source name to topics
+  lane: "lane_controller_node/car_cmd"
+  intersection: "open_loop_intersection_control_node/car_cmd"
+  # coordination: "simple_stop_controller_node/car_cmd"
+  joystick: "joy_mapper_node/car_cmd"
+  vehicle:  "vehicle_avoidance_control_node/car_cmd"
+  obstacle: "simple_stop_controller_node/car_cmd"
+  parallel_autonomy: "lane_supervisor_node/car_cmd"
+  recovery: "lane_recovery_node/car_cmd" # not implemented yet
+  deep_learning: "duckiebot_il_lane_following/car_cmd"
+  parking: "lane_controller_node/car_cmd"
+  stop: "simple_stop_controller_node/car_cmd"
+mappings: #Mapping from FSMStates.state to cmd source names. Allows different FSM mode to use the same source.
+  LANE_FOLLOWING: "lane" #TODO blackmagic
+  LANE_FOLLOWING_AVOID: "lane"
+  INTERSECTION_CONTROL: "intersection"
+  INTERSECTION_COORDINATION: "stop"
+  NORMAL_JOYSTICK_CONTROL: "joystick"
+  SAFE_JOYSTICK_CONTROL: "joystick"
+  AVOID_OBSTACLE: "obstacle"
+  AVOID_VEHICLE:    "vehicle"
+  LANE_RECOVERY:     "recovery"
+  INTERSECTION_RECOVERY: "recovery"
+  PARALLEL_AUTONOMY: "parallel_autonomy"
+  LOCALIZATION: "stop"
+  DEEP_LANE_FOLLOWING: "deep_learning"
+  PARKING: "parking"
+  ARRIVE_AT_STOP_LINE: "stop"

catkin_ws/src/00-infrastructure/duckietown/config/baseline/dagu_car/wheels_cmd_switch_node/default.yaml

@@ -1,7 +1,11 @@
 mode_topic: "fsm_node/mode"
 source_topics: #Mapping for name to topics
   lane: "lane_controller_node/wheels_control"
-  intersection: "open_loop_intersection_control_node/wheels_cmd"
+  # intersection: "intersection_navigation_node/cmds_out" #NOTE for open loop intersection navigation
+  intersection: "lane_controller_node/car_cmd" #NOTE for closed loop intersection navigation
+  planning: "intersection_navigation_node/cmds_out"
+  #NOTE for open_loop
+  # intersection: "open_loop_intersection_control_node/wheels_cmd"
   coordination: "veh_coordinator_node/coordination_cmd"
   joystick: 	"joy_mapper_node/car_cmd"
   vehicle: 	"vehicle_avoidance_control_node/wheels_cmd"
@@ -12,4 +16,5 @@ mappings: #Mapping from FSMStates.state to names
   COORDINATION: "coordination"
   JOYSTICK_CONTROL: "joystick"
   VEHICLE_AVOIDANCE: "vehicle"
-  OBSTACLE_AVOID:    "obstacle"
+  OBSTACLE_AVOID:    "obstacle"
+  INTERSECTION_PLANNING: "planning"

catkin_ws/src/00-infrastructure/duckietown/config/baseline/fleet_planning/actions_dispatcher_node/default.yaml

@@ -0,0 +1,6 @@
+
+initial_mode: JOYSTICK_CONTROL
+localization_mode: none
+trigger_mode: INTERSECTION_CONTROL
+reset_mode: JOYSTICK_CONTROL
+stop_line_wait_time: 2.0