Merge remote-tracking branch 'upstream/master'

Author: ponsheng

README.md

@@ -1,6 +1,14 @@
 #Xenobot
 
-A lightweight replica of MIT duckietown project.
+Inspired by MIT, this is a lightweight replica of MIT duckietown project.
+
+Our goal is to build a low cost self-driving car based on realtime Linux (patched by Xenomai)
+
+#Demo videos
+
+[![lane_following](https://github.com/ncku-ros2-research/xenobot/blob/master/materials/demo_video1.jpeg?raw=true)](https://www.youtube.com/watch?v=84MXc0_F61o)
+
+[![rviz](https://github.com/ncku-ros2-research/xenobot/blob/master/materials/demo_video2.jpeg?raw=true)](https://www.youtube.com/watch?v=XK602hzbORY&feature=youtu.be)
 
 ##Installation
 
@@ -62,6 +70,18 @@ roslaunch xenobot activate_controller.launch veh:=machine_name calibrate:=true
 roslaunch xenobot activate_controller.launch veh:=machine_name
 ```
 
+###4. Setup .bashrc
+
+Add:
+
+```
+. /opt/ros/kinetic/setup.bash
+. ~/catkin_ws/devel/setup.bash
+export ROS_IP=`hostname -I`
+alias play="roslaunch xenobot activate_controller.launch veh:=colin calibrate:=1"
+alias stop=". ~/catkin_ws/src/xenobot/halt_motor.sh"
+```
+
 ##Analysing
 
 ###Scatter plot

config/colin/pid.yaml

@@ -1,5 +1,5 @@
 pid_d:
-  kp: 3.5
+  kp: 3
   ki: 0
   kd: 0
 

materials/demo_video1.jpeg

@@ -67,10 +67,10 @@ void bound(float min, float max, float& x)
 	}
 }
 
-static float pid_d_control(int d_current, float d_setpoint, pid_control_t& pid)
+static float pid_control(int current_value, float setpoint, pid_control_t& pid)
 {
-	float p_term, d_term;
-	float current_error = d_current - d_setpoint;
+	float p_term, d_term, i_term;
+	float current_error = current_value - setpoint;
 
 	ros::Time current_time = ros::Time::now();
 
@@ -80,52 +80,25 @@ static float pid_d_control(int d_current, float d_setpoint, pid_control_t& pid)
 	if(pid.been_init == false) {
 		pid.been_init = true;
 		d_term = 0; //No D term
+		i_term = 0; //No I term
 	} else if(pid.been_halted == true) {
 		pid.been_halted = false;
 		d_term = 0; //No D term
+		i_term = 0; //No I term
 	} else {
 		//Calculate the time interval
 		float delta_t = current_time.toSec() - pid.previous_time.toSec();
 
-		//Calculate the D term
+		//Calculate I and D terms
 		d_term = pid.kd * (current_error - pid.previous_error) / delta_t;
+		i_term = pid.ki * current_error * delta_t;
 	}
 
 	pid.previous_error = current_error;
 	pid.previous_time = current_time;
+	pid.previous_i_term = i_term;
 
-	return p_term + d_term;
-}
-
-static float pid_phi_control(float phi_current, float phi_setpoint, pid_control_t& pid)
-{
-	float p_term, d_term;
-	float current_error = phi_current - phi_setpoint;
-
-	ros::Time current_time = ros::Time::now();
-
-	//Calculate the P term
-	p_term = pid.kp * current_error;
-
-	if(pid.been_init == false) {
-		pid.been_init = true;
-		d_term = 0; //No D term
-	} else if(pid.been_halted == true) {
-		pid.been_halted = false;
-		d_term = 0; //No D term
-	} else {
-		//Calculate the time interval
-		float delta_t = current_time.toSec() - pid.previous_time.toSec();
-
-		//Calculate the D term
-		d_term = pid.kd * (current_error - pid.previous_error) / delta_t;
-	}
-
-	pid.previous_error = current_error;
-	pid.previous_time = current_time;
-
-	return p_term + d_term;
-
+	return p_term + d_term + i_term;
 }
 
 void pid_halt()
@@ -138,11 +111,11 @@ void self_driving_controller(float d, float phi)
 {
 	int pwm_left, pwm_right;
 
-	float phi_setpoint = pid_d_control(d, 0, pid_d);
+	float phi_setpoint = pid_control(d, 0, pid_d);
 
 	bound(PHI_MIN, PHI_MAX, phi_setpoint);
 
-	int pwm = (int)pid_phi_control(phi, phi_setpoint, pid_phi);
+	int pwm = (int)pid_control(phi, phi_setpoint, pid_phi);
 
 	pwm_left = THROTTLE_BASE + pwm;
 	pwm_right = THROTTLE_BASE - pwm;

materials/demo_video2.jpeg

@@ -22,6 +22,7 @@ typedef struct {
 	bool been_init;
 
 	float previous_error;
+	float previous_i_term;
 
 	ros::Time previous_time; //XXX:This is not realtime!
 } pid_control_t;

system/controller.cpp

@@ -6,6 +6,7 @@
 #include <cv_bridge/cv_bridge.h>
 #include <yaml-cpp/yaml.h>
 
+#include <std_msgs/Float32.h>
 #include <xenobot/segment.h>
 #include <xenobot/segmentArray.h>
 
@@ -17,6 +18,7 @@
 #define DRAW_DEBUG_INFO 1
 
 #define rad_to_deg(phi) (phi * 57.2957795)
+#define PI 3.141592
 
 using namespace std;
 using namespace cv;
@@ -57,7 +59,13 @@ LaneDetector::LaneDetector(string _yaml_path, bool calibrate_mode) :
 			node.advertise<sensor_msgs::Image>("xenobot/bird_view_image", 10);
 
 		histogram_publisher =
-			node.advertise<xenobot::segmentArray>("/xenobot/segment_data", 10);;
+			node.advertise<xenobot::segmentArray>("/xenobot/segment_data", 10);
+
+		pose_d_publisher =
+			node.advertise<std_msgs::Float32>("/xenobot/pose/d", 10);
+
+		pose_phi_publisher =
+			node.advertise<std_msgs::Float32>("/xenobot/pose/phi", 10);
 	}
 }
 
@@ -482,6 +490,12 @@ void LaneDetector::send_sucess_visualize_image_thread(
 
 	histogram_publisher.publish(segments_msg);
 
+	std_msgs::Float32 pose_msg;
+	pose_msg.data = d;
+	pose_d_publisher.publish(pose_msg);
+	pose_msg.data = phi;
+	pose_phi_publisher.publish(pose_msg);
+
 	ROS_INFO("phi:%f | d:%f", phi, d);
 }
 
@@ -509,8 +523,6 @@ void LaneDetector::send_failed_visualize_image_thread(
 		outer_threshold_image, inner_threshold_image,
 		bird_view_image
 	);
-
-	ROS_INFO("Failed to estimate the lane");
 }
 
 
@@ -616,7 +628,7 @@ bool LaneDetector::lane_estimate(cv::Mat& raw_image, float& final_d, float& fina
 			inner_threshold_image
 		);
 
-		ROS_INFO("[No segment found]");
+		ROS_INFO("Failed to estimate the lane [No segment found]");
 
 		return false;
 	}
@@ -728,7 +740,7 @@ bool LaneDetector::lane_estimate(cv::Mat& raw_image, float& final_d, float& fina
 			inner_threshold_image
 		);
 
-		ROS_INFO("[Less than threshold value]");
+		ROS_INFO("Failed to estimate the lane [Less than threshold value]");
 
 		return false;
 	}
@@ -789,7 +801,7 @@ bool LaneDetector::lane_estimate(cv::Mat& raw_image, float& final_d, float& fina
 			inner_threshold_image
 		);
 
-		ROS_INFO("Sample count equals zero");
+		ROS_INFO("Failed to estimate the lane [Sample count equals zero]");
 
 		return false;
 	}
@@ -856,31 +868,45 @@ bool LaneDetector::generate_vote(segment_t& lane_segment, float& d,
 	normalize(t_hat);
 
 	/* Estimate phi */
-	phi = rad_to_deg(atan2f(t_hat.y, t_hat.x)) + 90.0f;
+	phi = atan2f(t_hat.y, t_hat.x) + PI / 2;
 
 	Point2f n_hat(-t_hat.y, t_hat.x); //normal vector
 
-	float d1 = inner_product(n_hat, p1);
-	float d2 = inner_product(n_hat, p2);
+	Point2f offset_vector = n_hat;
 
-	d = (d1 + d2) / 2; //lateral displacement
+	float steady_bias = 4; //cm
 
 	if(color == WHITE) {
-		d -= W / 2;
-
 		if(lane_segment.side == RIGHT_EDGE) {
-			d -= L_W;
+			offset_vector *= -(W / 2) - L_W;
+		} else {
+			offset_vector *= -(W / 2);
 		}
 	} else if(color == YELLOW) {
-		d += W / 2;
-
 		if(lane_segment.side == LEFT_EDGE) {
-			d += L_Y;
+			offset_vector *= +(W / 2) + L_Y + steady_bias;
+		} else {
+			offset_vector *= +(W / 2) + steady_bias;
 		}
 	}
 
+	p1 += offset_vector;
+	p2 += offset_vector;
+
+	p1.x -= CAMERA_TO_CENTER * sin(phi);
+	p1.y -= CAMERA_TO_CENTER * cos(phi);
+	p2.x -= CAMERA_TO_CENTER * sin(phi);
+	p2.y -= CAMERA_TO_CENTER * cos(phi);
+
+	float d1 = inner_product(n_hat, p1);
+	float d2 = inner_product(n_hat, p2);
+
+	d = (d1 + d2) / 2; //lateral displacement
+
 	//TODO:referive the geometry formulas!
 	d *= -1;
 
+	phi = rad_to_deg(phi);
+
 	return true;
 }

system/controller.hpp

@@ -27,6 +27,9 @@ using namespace std;
 #define L_Y 2.5
 #define W 16.5
 
+/* Car parameter */
+#define CAMERA_TO_CENTER 10.0 //cm
+
 /* Historgram filter parameters */
 #define DELTA_PHI 2.0 //degree
 #define DELTA_D 2.0 //cm
@@ -40,7 +43,7 @@ using namespace std;
 
 /* Lane detector parameter */
 #define CANNY_THRESHOLD_1 50
-#define CANNY_THRESHOLD_2 300
+#define CANNY_THRESHOLD_2 200
 #define HOUGH_THRESHOLD 50
 #define SIDE_DETECT_PIXEL_CNT 20
 #define SIDE_DETECT_THREDHOLD 14
@@ -103,6 +106,8 @@ class LaneDetector {
 	ros::Publisher marked_image_publisher;
 	ros::Publisher bird_view_img_publisher;
 	ros::Publisher histogram_publisher;
+	ros::Publisher pose_d_publisher;
+	ros::Publisher pose_phi_publisher;
 
 	bool read_threshold_setting(string yaml_path);
 	bool read_extrinsic_calibration(string yaml_path);