change src/ros/catkin. for test pure_pusuit.

src/driver/driver_autoware_computewaypoints/computewaypoints.cpp

@@ -89,8 +89,8 @@ void ComputeWayPoints::ListenNewTraceMap(const char *strdata, const unsigned int
     (void)dt;
     (void)strmemname;
 
-    const double fspeedlimt = 30.0/3.6;
-    const double fdacc = -0.5;
+    const double fspeedlimt = 30.0/3.6;   //Speeed Limit
+    const double fdacc = -0.5;   //Acc Limit
 
     iv::map::tracemap xtracemap;
     if(!xtracemap.ParseFromArray(strdata,nSize))
@@ -136,9 +136,12 @@ int  ComputeWayPoints::FindWayPoints(iv::map::tracemap &xtracemap)
     double fheaddiff;
     int nnearindex = -1;
     double fdismin = std::numeric_limits<double>::max();
+    const double facclimithigh = 1.5;   //Acc Max
+    const double facclimitlow = -9.0;   //Acc Min
 
     double fnowx,fnowy;
     GaussProjCal(xgpsimu.lon(),xgpsimu.lat(),&fnowx,&fnowy);
+    double fnowspeed = sqrt(pow(xgpsimu.ve(),2) + pow(xgpsimu.vn(),2));
 
     int i;
     int ntracesize = mtracemap.point_size();
@@ -209,12 +212,59 @@ int  ComputeWayPoints::FindWayPoints(iv::map::tracemap &xtracemap)
         return 0;
     }
 
+
+    //Get Point Adn fix speed.
     int j = 0;
+    double flastx = fnowx;
+    double flasty = fnowy;
+    double flastspeed = fnowspeed;
     for(i=nnearindex;i<ntracesize;i++)
     {
         iv::map::mappoint * pnew = xtracemap.add_point();
         pnew->CopyFrom(mtracemap.point(i));
         j++;
+        if(j== 1)
+        {
+//            pnew->set_speed(fnowspeed);
+        }
+        else
+        {
+            double fdis = sqrt(pow(pnew->gps_x() - flastx,2)+pow(pnew->gps_y() - flasty,2) );
+            if(fdis > 0)
+            {
+                if((pnew->speed() > 1.0)&&(j==2))
+                {
+                    //Allow 5% speed diff
+//                    if(fabs(pnew->speed() - fnowspeed)<(fnowspeed*0.05))
+//                    {
+//                        pnew->set_speed(fnowspeed);
+//                    }
+//                    else
+//                    {
+//                        double vmax = sqrt(flastspeed*flastspeed + 2*facclimithigh*fdis);
+//                        double fvalue = flastspeed+flastspeed + 2*facclimitlow*fdis;
+//                        double vmin = 0;
+//                        if(fvalue>0)vmin = sqrt(fvalue);
+//                        std::cout<<" vmax: "<<vmax<<" vmin: "<<vmin<<std::endl;
+//                        if(pnew->speed()>vmax)pnew->set_speed(vmax);
+//                        if(pnew->speed()<vmin)pnew->set_speed(vmin);
+//                    }
+                }
+                else
+                {
+//                    double vmax = sqrt(flastspeed*flastspeed + 2*facclimithigh*fdis);
+//                    double fvalue = flastspeed+flastspeed + 2*facclimitlow*fdis;
+//                    double vmin = 0;
+//                    if(fvalue>0)vmin = sqrt(fvalue);
+//                    if(pnew->speed()>vmax)pnew->set_speed(vmax);
+//                    if(pnew->speed()<vmin)pnew->set_speed(vmin);
+                }
+            }
+
+            flastspeed = pnew->speed();
+            flastx = pnew->gps_x();
+            flasty = pnew->gps_y();
+        }
         if(j>1000)break;
     }
 

src/driver/driver_odomtogpsimu/main.cpp

@@ -171,12 +171,12 @@ void ListenODOM(const char * strdata,const unsigned int nSize,const unsigned int
 void GetParam(std::string  strpath)
 {
     iv::xmlparam::Xmlparam xp(strpath);
-    glon0 = xp.GetParam("lon0",117.3548316);
-    glat0 = xp.GetParam("lat0",39.0677445);
+    glon0 = xp.GetParam("lon0",117.02802269);
+    glat0 = xp.GetParam("lat0",39.120177699);
     gstrmsg_gpsimu = xp.GetParam("gpsmsgname","hcp2_gpsimu");
     gstrmsg_odom = xp.GetParam("odommsgname","odom_ros");
-    gfix_x = xp.GetParam("fix_x",-13.0);
-    gfix_y = xp.GetParam("fix_y",4.0);
+    gfix_x = xp.GetParam("fix_x",2.5);;// xp.GetParam("fix_x",-13.0);
+    gfix_y = xp.GetParam("fix_y",-2.5);//xp.GetParam("fix_y",4.0);
     ghead0 = 360.0;
     gheight0 = 0.0;
 }

src/ros/catkin/launch/pure_pursuit_withpilot.launch

@@ -0,0 +1,37 @@
+<!-- -->
+<launch>
+  <arg name="is_linear_interpolation" default="True"/>
+  <arg name="publishes_for_steering_robot" default="True"/>
+  <arg name="add_virtual_end_waypoints" default="False"/>
+  <arg name="const_lookahead_distance" default="4.0"/>
+  <arg name="const_velocity" default="5.0"/>
+  <arg name="lookahead_ratio" default="2.0"/>
+  <arg name="minimum_lookahead_distance" default="6.0"/>
+
+  <!-- 0 = waypoints, 1 = provided constant velocity -->
+  <arg name="velocity_source" default="0"/>
+
+  <!-- rosrun waypoint_follower pure_pursuit -->
+  <node pkg="pure_pursuit" type="pure_pursuit" name="pure_pursuit" output="log">
+    <param name="is_linear_interpolation" value="$(arg is_linear_interpolation)"/>
+    <param name="publishes_for_steering_robot" value="$(arg publishes_for_steering_robot)"/>
+    <param name="add_virtual_end_waypoints" value="$(arg add_virtual_end_waypoints)"/>
+    <param name="const_lookahead_distance" value="$(arg const_lookahead_distance)"/>
+    <param name="const_velocity" value="$(arg const_velocity)"/>
+    <param name="lookahead_ratio" value="$(arg lookahead_ratio)"/>
+    <param name="minimum_lookahead_distance" value="$(arg minimum_lookahead_distance)"/>
+    <param name="velocity_source" value="$(arg velocity_source)"/>
+  </node>
+  <node pkg="pilottoros" type="pilottoros_node" name="pilottoros"  output="screen">
+    <param name="points_node" value="points_raw"/>
+    <param name="points_msgname" value="lidar_pc"/>
+    <param name="image_node" value="image_raw"/>
+    <param name="image_msgname" value="picfront"/>
+    <param name="waypoints_node" value="/final_waypoints"/>
+    <param name="waypoints_msgname" value="waypoints"/>
+    <param name="use_rtk_odom" value="False"/>
+    <param name="use_pilot_waypoints" value="True"/>    
+  </node>
+  <node pkg="rostopilot" type="rostopilot_node" name="rostopilot"  output="screen"> 
+  </node>
+</launch>

src/ros/catkin/src/pilottoros/src/main.cpp

@@ -170,8 +170,10 @@ void Listenodom(const char * strdata,const unsigned int nSize,const unsigned int
     geometry_msgs::TwistStamped xtwist;
     xtwist.twist = msg.twist.twist;
 
+std::cout<<" listen odom."<<std::endl;
 if(_use_pilot_waypoints)
 {
+    std::cout<<" public pose"<<std::endl;
     pose_pub.publish(xPose);
     twist_pub.publish(xtwist);
 }
@@ -338,6 +340,16 @@ int main(int argc, char *argv[])
     private_nh.getParam("use_rtk_odom",_use_rtk_odom);
     private_nh.getParam("use_pilot_waypoints",_use_pilot_waypoints);
 
+    if(_use_rtk_odom == false)
+    {
+        std::cout<<"not use rtk. "<<std::endl;
+    }
+
+    if(_use_pilot_waypoints)
+    {
+        std::cout<<" use pilot calculate waypoints."<<std::endl;
+    }
+
     point_cloud_pub = private_nh.advertise<sensor_msgs::PointCloud2>(
                 _point_topic, 10);
 

src/ros/catkin/src/pure_pursuit/launch/pure_pursuit.launch

@@ -1,7 +1,7 @@
 <!-- -->
 <launch>
   <arg name="is_linear_interpolation" default="True"/>
-  <arg name="publishes_for_steering_robot" default="False"/>
+  <arg name="publishes_for_steering_robot" default="True"/>
   <arg name="add_virtual_end_waypoints" default="False"/>
   <arg name="const_lookahead_distance" default="4.0"/>
   <arg name="const_velocity" default="5.0"/>

src/ros/catkin/src/pure_pursuit/src/pure_pursuit_core.cpp

@@ -55,7 +55,7 @@ void PurePursuitNode::initForROS()
   private_nh_.param("velocity_source", velocity_source_, 0);
   private_nh_.param("is_linear_interpolation", is_linear_interpolation_, true);
   private_nh_.param(
-    "publishes_for_steering_robot", publishes_for_steering_robot_, false);
+    "publishes_for_steering_robot", publishes_for_steering_robot_, true);
   private_nh_.param(
     "add_virtual_end_waypoints", add_virtual_end_waypoints_, false);
   private_nh_.param("const_lookahead_distance", const_lookahead_distance_, 4.0);
@@ -227,6 +227,7 @@ double PurePursuitNode::computeCommandAccel() const
         current_pose.position.y - target_pose.position.y);
   const double v0 = current_linear_velocity_;
   const double v = computeCommandVelocity();
+  ROS_WARN(" v: %lf  v0:%lf  x:%lf",v,v0,x);
   const double a = getSgn() * (v * v - v0 * v0) / (2 * x);
   return a;
 }

src/ros/catkin/src/rostopilot/src/main.cpp

@@ -22,6 +22,8 @@
 #include "lgsvl_msgs/VehicleControlData.h"
 #include "lgsvl_msgs/VehicleStateData.h"
 
+#include  <geometry_msgs/TwistStamped.h>
+
 #include "rawpic.pb.h"
 #include "odom.pb.h"
 #include "decition.pb.h"
@@ -35,10 +37,13 @@ static std::string _image_msgname = "pic_ros";
 static std::string  _odom_topic = "/odom";
 static std::string _odom_msgname = "odom_ros";
 
+static std::string  _twistraw_topic = "/twist_raw";
 
   ros::Subscriber points_sub;
   ros::Subscriber odom_sub;
 
+  ros::Subscriber twist_sub;
+
   void * gpa_lidar;
   void * gpa_image;
   void * gpa_odom;
@@ -203,6 +208,12 @@ void testvh()
      }
 }
 
+void  twistrawCalllback(const  geometry_msgs::TwistStampedConstPtr  & msg)
+{
+    autoware_msgs::VehicleCmd xcmd;
+    xcmd.twist_cmd.twist = msg->twist;
+    test_cmd_pub.publish(xcmd);
+}
 void UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
 
@@ -290,6 +301,9 @@ int main(int argc, char *argv[])
     std::cout<<"_odom_topic is "<<_odom_topic<<std::endl;
 	std::cout<<"_odom_msgname : "<<_odom_msgname<<std::endl;
 
+    private_nh.getParam("autoware_twist_raw",_twistraw_topic);
+    std::cout<<"  _twist_topic : "<<_twistraw_topic<<std::endl;
+
     test_cmd_pub =  private_nh.advertise<autoware_msgs::VehicleCmd>(
                 "/vehicle_cmd", 10);
     lgsvl_state_pub = private_nh.advertise<lgsvl_msgs::VehicleStateData>("/vehicle_state",10);
@@ -306,6 +320,8 @@ int main(int argc, char *argv[])
 
     odom_sub = private_nh.subscribe(_odom_topic,1,odomCalllback);
 
+twist_sub = private_nh.subscribe(_twistraw_topic,1,twistrawCalllback);
+
    void * pdecition = iv::modulecomm::RegisterRecv("deciton",UpdateDecition);
   //   std::thread * pnew = new std::thread(testvh);