Commit dc46cbcf authored by Svetlana's avatar Svetlana

Working steering control

parent 185a79a9
......@@ -54,7 +54,7 @@ public class Generator {
FileUtils.copyDirectory(new File(RESOURCES_PATH+"torcs"), new File(TARGET_PATH_GENERATED+"torcs"));
Runtime.getRuntime().exec("chmod u+x "+TARGET_PATH_GENERATED+"torcs/build.sh");
FileUtils.copyDirectory(new File(RESOURCES_PATH+"dpnet"), new File(TARGET_PATH_GENERATED+"model/dp_mastercomponent_dpnet"));
FileUtils.copyDirectory(new File(RESOURCES_PATH+"dpnet"), new File(TARGET_PATH_GENERATED+"model"));
} catch (IOException e) {
System.err.println("Failed to copy build and start scripts.");
e.printStackTrace();
......
......@@ -3,9 +3,9 @@ import dp.subcomponents.*;
component Mastercomponent {
ports
in Z(0:255)^{3, 210, 280} imageIn,
in Q(0:255)^{3, 210, 280} imageIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(0:1)^{3} commandsOut;
out Q(-1:1)^{3} commandsOut;
instance Dpnet dpnet;
instance Drivercontroller driverController;
......
......@@ -2,7 +2,7 @@
package dp.subcomponents;
component Dpnet{
ports in Z(0:255)^{3, 210, 280} image,
ports in Q(0:255)^{3, 210, 280} image,
out Q(-oo:oo)^{14} predictions;
implementation CNN {
......@@ -26,6 +26,9 @@ component Dpnet{
conv(kernel=(3,3), channels=256) ->
fc() ->
fc() ->
FullyConnected(units=256) ->
Relu() ->
Dropout() ->
FullyConnected(units=14, no_bias=true) ->
predictions
}
......
......@@ -5,7 +5,7 @@ component Drivercontroller {
in Q^{5} anglesIn,
in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(0:1)^{3} commandsOut;
out Q(-1:1)^{3} commandsOut;
implementation Math {
Q roadWidth = 8;
......@@ -45,13 +45,18 @@ component Drivercontroller {
coeSteer = 0.3;
end
Q steerCmd = 0;
// steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
commandsOut(2) = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer;
steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052/coeSteer;
if (coeSteer > 1 && steerCmd > 0.1) // reshape the steering control curve
steerCmd = steerCmd * (2.5 * steerCmd + 0.75);
end
commandsOut(2) = steerCmd;
Q desiredSpeed = 20;
if (affordanceIn.fast != 1)
desiredSpeed = 20 - abs(anglesIn(0) + anglesIn(1) + anglesIn(2) + anglesIn(3) + anglesIn(4)) * 4.5;
end
//if (affordanceIn.fast != 1)
// desiredSpeed = 20 - abs(anglesIn(0) + anglesIn(1) + anglesIn(2) + anglesIn(3) + anglesIn(4)) * 4.5;
//end
if (desiredSpeed < 10)
desiredSpeed = 10;
......
......@@ -49,23 +49,14 @@ int main(int argc, char **argv)
cv::cvtColor(flippedImage, rgbImage, cv::COLOR_RGB2BGR);
//Code for debugging: displays screenshot in a window and waits for a key stroke to close it and get new one
//cv::namedWindow("Display Image", cv::WINDOW_AUTOSIZE );
//cv::imshow("Display Image", rgbImage);
//cv::waitKey(0);
//cv::destroyWindow("Display Image");
// std::vector<float> data(3*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH);
// for(size_t j=0; j<ROS_IMAGE_HEIGHT; j++){
// for(size_t k=0; k<ROS_IMAGE_WIDTH; k++){
// cv::Vec3b intensity = rgbImage.at<cv::Vec3b>(j, k);
// for(size_t i=0; i<3; i++){
// data[i*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH + j*ROS_IMAGE_HEIGHT + k] = (float) intensity[i];
// }
// }
// }
// cv::namedWindow("Display Image 1", cv::WINDOW_AUTOSIZE );
// cv::imshow("Display Image 1", rgbImage);
// cv::waitKey(0);
// cv::destroyWindow("Display Image 1");
std::vector<float> data(0);
data.reserve(3*ROS_IMAGE_HEIGHT*ROS_IMAGE_WIDTH);
data.reserve(ROS_IMAGE_SIZE);
for(size_t j=0; j<ROS_IMAGE_HEIGHT; j++){
for(size_t k=0; k<ROS_IMAGE_WIDTH; k++){
cv::Vec3b intensity = rgbImage.at<cv::Vec3b>(j, k);
......@@ -75,10 +66,11 @@ int main(int argc, char **argv)
}
}
assert(sizeof(float) == sizeof(std_msgs::Float32));
std_msgs::Float32MultiArray::Ptr msg(new std_msgs::Float32MultiArray);
msg->data.resize(ROS_IMAGE_SIZE);
memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE);
memcpy(msg->data.data(), data.data(), ROS_IMAGE_SIZE*sizeof(float));
std_msgs::Float32::Ptr speedMsg(new std_msgs::Float32);
speedMsg->data = speed;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment