From 57be11dc51e1ccaab1786718938a8a5b409c1c96 Mon Sep 17 00:00:00 2001 From: ibrahim-mo Date: Mon, 18 Dec 2017 19:20:21 -0800 Subject: [PATCH] Include udacity master branch updates + update .gitignore --- .gitignore | 3 +++ README.md | 2 +- ros/src/styx/bridge.py | 18 +++++++++++++++++- ros/src/styx/conf.py | 1 + ros/src/styx/server.py | 4 +++- twist_controller.py | 41 ----------------------------------------- 6 files changed, 25 insertions(+), 44 deletions(-) delete mode 100644 twist_controller.py diff --git a/.gitignore b/.gitignore index 663d56dd82..c421a78207 100755 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,6 @@ build devel profile.tmp + +.DS_Store +.idea diff --git a/README.md b/README.md index 9eba41330d..bd6b33c9c0 100755 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ This is the project repo for the final project of the Udacity Self-Driving Car N * [ROS Indigo](http://wiki.ros.org/indigo/Installation/Ubuntu) if you have Ubuntu 14.04. * [Dataspeed DBW](https://bitbucket.org/DataspeedInc/dbw_mkz_ros) * Use this option to install the SDK on a workstation that already has ROS installed: [One Line SDK Install (binary)](https://bitbucket.org/DataspeedInc/dbw_mkz_ros/src/81e63fcc335d7b64139d7482017d6a97b405e250/ROS_SETUP.md?fileviewer=file-view-default) -* Download the [Udacity Simulator](https://github.com/udacity/CarND-Capstone/releases/tag/v1.2). +* Download the [Udacity Simulator](https://github.com/udacity/CarND-Capstone/releases). ### Docker Installation [Install Docker](https://docs.docker.com/engine/installation/) diff --git a/ros/src/styx/bridge.py b/ros/src/styx/bridge.py index b3c7e39243..b2874d7677 100755 --- a/ros/src/styx/bridge.py +++ b/ros/src/styx/bridge.py @@ -12,7 +12,7 @@ from std_msgs.msg import Header from cv_bridge import CvBridge, CvBridgeError -from styx_msgs.msg import TrafficLight, TrafficLightArray +from styx_msgs.msg import TrafficLight, TrafficLightArray, Lane import numpy as np from PIL import Image as PIL_Image from io import BytesIO @@ -31,6 +31,7 @@ 'steer_cmd': SteeringCmd, 'brake_cmd': BrakeCmd, 'throttle_cmd': ThrottleCmd, + 'path_draw': Lane, 'image':Image } @@ -48,6 +49,7 @@ def __init__(self, conf, server): '/vehicle/steering_cmd': self.callback_steering, '/vehicle/throttle_cmd': self.callback_throttle, '/vehicle/brake_cmd': self.callback_brake, + '/final_waypoints': self.callback_path } self.subscribers = [rospy.Subscriber(e.topic, TYPE[e.type], self.callbacks[e.topic]) @@ -188,3 +190,17 @@ def callback_throttle(self, data): def callback_brake(self, data): self.server('brake', data={'brake': str(data.pedal_cmd)}) + + def callback_path(self, data): + x_values = [] + y_values = [] + z_values = [] + for waypoint in data.waypoints: + x = waypoint.pose.pose.position.x + y = waypoint.pose.pose.position.y + z = waypoint.pose.pose.position.z+0.5 + x_values.append(x) + y_values.append(y) + z_values.append(z) + + self.server('drawline', data={'next_x': x_values, 'next_y': y_values, 'next_z': z_values}) diff --git a/ros/src/styx/conf.py b/ros/src/styx/conf.py index 594b8f3468..408dc0952d 100755 --- a/ros/src/styx/conf.py +++ b/ros/src/styx/conf.py @@ -5,6 +5,7 @@ {'topic':'/vehicle/steering_cmd', 'type': 'steer_cmd', 'name': 'steering'}, {'topic':'/vehicle/throttle_cmd', 'type': 'throttle_cmd', 'name': 'throttle'}, {'topic':'/vehicle/brake_cmd', 'type': 'brake_cmd', 'name': 'brake'}, + {'topic':'/final_waypoints', 'type': 'path_draw', 'name': 'path'}, ], 'publishers': [ {'topic': '/current_pose', 'type': 'pose', 'name': 'current_pose'}, diff --git a/ros/src/styx/server.py b/ros/src/styx/server.py index 584563e2b5..5a94ac0088 100755 --- a/ros/src/styx/server.py +++ b/ros/src/styx/server.py @@ -1,8 +1,10 @@ #!/usr/bin/env python -import socketio import eventlet +eventlet.monkey_patch(socket=True, select=True, time=True) + import eventlet.wsgi +import socketio import time from flask import Flask, render_template diff --git a/twist_controller.py b/twist_controller.py deleted file mode 100644 index 9405f8825b..0000000000 --- a/twist_controller.py +++ /dev/null @@ -1,41 +0,0 @@ -from yaw_controller import YawController -from pid import PID - -GAS_DENSITY = 2.858 -ONE_MPH = 0.44704 -CMD_RATE = 50 - - -class Controller(object): - def __init__(self, wheel_base, steer_ratio, max_lat_accel, max_steer_angle): - self._wheel_base = wheel_base - self._steer_ratio = steer_ratio - self._max_lat_accel = max_lat_accel - self._max_steer_angle = max_steer_angle - - def control(self, trgt_lin_vel, trgt_ang_vel, curr_lin_vel, dbw_enabled): - pid = PID(kp = 1.0, ki = 0.5, kd = 0.0, mn = -5.0, mx = 5.0) - err = trgt_lin_vel - curr_lin_vel - dt = 1.0 / CMD_RATE - acc = pid.step(err, dt) - - # If dbw was disabled - if not dbw_enabled: - pid.reset() - return 0., 0., 0. - - yaw_controller = YawController(self._wheel_base, self._steer_ratio, ONE_MPH, - self._max_lat_accel, self._max_steer_angle) - steer = yaw_controller.get_steering(trgt_lin_vel, trgt_ang_vel, curr_lin_vel) - - ### May need to add a low pass filter to acc if jerk becomes and issue. - - throttle = 0.0 - brake = 0.0 - if acc > 0.0: - throttle = acc / 5.0 - if acc <= 0.0: - # brake = acc * '~vehicle_mass' * '~wheel_radius' 1736.35*.2413 - brake = -1.0 * acc * 1736.35 * .2413 - - return throttle, brake, steer