Merge pull request #5 from qingjieqi/master

Path following launch
JuliaMPC · Nov 2, 2018 · 1887506 · 1887506
2 parents 29dd5fc + a91060e
commit 1887506
Show file tree

Hide file tree

Showing 5 changed files with 62 additions and 46 deletions.
diff --git a/ros/src/models/chrono/ros_chrono/launch/path_follower.launch b/ros/src/models/chrono/ros_chrono/launch/path_follower.launch
@@ -1,29 +1,14 @@
 <launch>
   <arg name="system_params_path" default="$(find system)/config/system/pkgs/chrono.yaml"/>
   <arg name="vehicle_params_path" default="$(find system)/config/vehicle/hmmwv.yaml"/>
-  <arg name="case_params_path" default="$(find system)/config/case/s1.yaml"/>
+  <arg name="case_params_path" default="$(find system)/config/case/path_follower.yaml"/>
   <arg name="chrono_params_path" default="$(find system)/config/planner/chrono_testing/hmmwv_chrono_params.yaml"/>
 
-
-
   <rosparam command="load" file="$(arg system_params_path)"/>
   <rosparam command="load" file="$(arg vehicle_params_path)"/>
   <rosparam command="load" file="$(arg case_params_path)"/>
   <rosparam command="load" file="$(arg chrono_params_path)"/>
-
-  <!--rosparam command="load" file="$(find system)/config/case/case1.yaml"/>
-  <rosparam command="load" file="$(find system)/config/system/demoD.yaml"/>
-  <rosparam command="load" file="$(find system)/config/system/test_chrono.yaml"/>
-  <rosparam command="load" file="$(find system)/config/vehicle/hmmwv.yaml"/>
-  <node name="Reference" pkg="traj_gen_chrono" type="traj_gen_chrono"/>
-  <node name="Chronode" pkg="ros_chrono" type="hmmwv_model" cwd="node" />
-  <node name="tf_position_broadcaster" pkg="chrono_position_broadcaster" type="chrono_position_broadcaster" cwd="node" /-->
-
-  <include file="$(find ros_chrono)/launch/main.launch">
-    <arg name="system_params_path" value="$(arg system_params_path)"/>
-    <arg name="vehicle_params_path" value="$(arg vehicle_params_path)"/>
-    <arg name="case_params_path" value="$(arg case_params_path)"/>
-    <arg name="chrono_params_path" value="$(arg chrono_params_path)"/>
-
-  </include>
+
+  <node name="path" pkg="traj_gen_chrono" type="path_gen_chrono" output="screen"/>
+  <node name="Chronode" pkg="ros_chrono" type="path_follower" cwd="node" />
 </launch>
diff --git a/ros/src/models/chrono/ros_chrono/src/data/vehicle/paths/switch_lane.txt b/ros/src/models/chrono/ros_chrono/src/data/vehicle/paths/switch_lane.txt
@@ -1,5 +1,5 @@
 4 3
-     56.0000    -125.0000       0.1000
-   -125.0000    -125.0000       0.1000
-   -125.0000    -120.0000       0.1000
+     56.0000    -122.0000       0.1000
+   -122.0000    -122.0000       0.1000
+   -122.0000    -120.0000       0.1000
      56.0000    -120.0000       0.1000
diff --git a/ros/src/models/chrono/ros_chrono/src/path_follower.cpp b/ros/src/models/chrono/ros_chrono/src/path_follower.cpp
@@ -58,7 +58,7 @@ using namespace alglib;
 
 const double PI  = 3.14159265359;
 
-std::string data_path("MAVs/ros/src/models/chrono/ros_chrono/src/data/vehicle/");
+std::string data_path("../../../src/models/chrono/ros_chrono/src/data/vehicle/");
 
 // The extended steering controller only works inside the path limits
 // =============================================================================
@@ -196,9 +196,13 @@ int main(int argc, char* argv[]) {
     // ------------------------------
     ros::init(argc, argv, "steering_controller");
     ros::NodeHandle node;
+
 
     // Declare ROS subscriber to subscribe planner topic
-    ros::Subscriber planner_sub = node.subscribe("/control", 100, plannerCallback);
+    std::string planner_namespace;
+    node.getParam("system/planner",planner_namespace);
+    ros::Subscriber planner_sub = node.subscribe(planner_namespace + "/control", 100, plannerCallback);
+    //ros::Subscriber planner_sub = node.subscribe("/control", 100, plannerCallback);
 
     // Declare ROS publisher to advertise vehicleinfo topic
     ros::Publisher vehicleinfo_pub = node.advertise<ros_chrono_msgs::veh_status>("/vehicleinfo", 1);

diff --git a/ros/src/models/chrono/traj_gen_chrono/src/path_gen_chrono.cpp b/ros/src/models/chrono/traj_gen_chrono/src/path_gen_chrono.cpp
@@ -8,8 +8,6 @@
 #include "chrono/core/ChFileutils.h"
 #include "chrono_vehicle/utils/ChVehiclePath.h"
 
-std::string path_file("MAVs/ros/src/models/chrono/ros_chrono/src/data/vehicle/paths/ISO_double_lane_change.txt");
-
 using namespace chrono;
 
 int main(int argc, char **argv) {
@@ -19,9 +17,11 @@ int main(int argc, char **argv) {
     // create node handle
     ros::NodeHandle node;
 
-    ros::Publisher pub = node.advertise<nloptcontrol_planner::Control>("/control", 10);
-
-    auto path = ChBezierCurve::read(path_file);
+    std::string planner_namespace;
+    node.getParam("system/planner",planner_namespace);
+    ros::Publisher pub = node.advertise<nloptcontrol_planner::Control>(planner_namespace + "/control", 10);
+    // ros::Publisher pub = node.advertise<nloptcontrol_planner::Control>("/control", 10);
+
     nloptcontrol_planner::Control control_info;
     int control_num = 2;
     std::vector<double> control_t(control_num,0.0);
@@ -38,28 +38,20 @@ int main(int argc, char **argv) {
 
     int count = 0;
     while (ros::ok()) {
-        ROS_INFO("Hello world");
-
-        // if (count < path->getNumPoints()-1) {
-        //     ChVector<> pos_1 = path->getPoint(count);
-        //     ChVector<> pos_2 = path->getPoint(count+1);
-
-        //     control_info.x = {pos_1[0], pos_2[0]};
-        //     control_info.y = {pos_1[1], pos_2[1]};
-
-        //     count++;
-        // }
-
         count++;
-        if (count % 2)
-            control_info.y =  {-125.0000, -125.0000};
-        else
+        if (count % 2){
+            ROS_INFO("Swithed to right lane");
             control_info.y =  {-120.0000, -120.0000};
+        }
+        else {
+            ROS_INFO("Swithed to left lane");
+            control_info.y =  {-122.0000, -122.0000};
+        }
 
-        control_info.vx[0] = 12;
+        control_info.vx[0] = 10;
+        pub.publish(control_info);
 
         ros::spinOnce();
-        pub.publish(control_info);
         loop_rate.sleep();
     }
     return 0;

diff --git a/ros/src/system/config/case/path_follower.yaml b/ros/src/system/config/case/path_follower.yaml
@@ -0,0 +1,35 @@
+case:
+ goal:
+  x: 200.
+  yVal: 125.
+  psi: 1.5701
+  tol: 4
+
+ actual:
+  X0:
+   x: -115.0
+   yVal: -120.0
+   v: 0.0
+   r: 0.0
+   psi: 1.57079632679
+   sa: 0.0
+   ux: 0.0
+   ax: 0.0
+  obstacle:
+   num: 1
+   radius: [1.]
+   length: [5.]
+   x0: [200]
+   y0: [50]
+   vx: [0]
+   vy: [0]
+
+ assumed:
+  obstacle:
+   num: 10
+   radius: [1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0]
+   length: [4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.0,4.0]
+   x0: [-100.0,-100.0,-100.0,-100.0,-100.0,-100.0,-100.0,-100.0,-100.0,-100.0]
+   y0: [0,2,4,6,8,10,12,14,16,18]
+   vx: [0,0,0,0,0,0,0,0,0,0]
+   vy: [0,0,0,0,0,0,0,0,0,0]