change pilottoros. for test purepuit.

src/driver/driver_autoware_computewaypoints/computewaypoints.cpp

@@ -201,7 +201,7 @@ int  ComputeWayPoints::FindWayPoints(iv::map::tracemap &xtracemap)
     }
 
 
-//    std::cout<<" fdismin : "<<fdismin<<std::endl;
+    std::cout<<" fdismin : "<<fdismin<<std::endl;
 
     if(fdismin > 10.0)
     {
@@ -219,7 +219,7 @@ int  ComputeWayPoints::FindWayPoints(iv::map::tracemap &xtracemap)
     }
 
 
-//    std::cout<<" waypoint size : "<<xtracemap.point_size()<<std::endl;
+    std::cout<<" waypoint size : "<<xtracemap.point_size()<<std::endl;
      mMutexTraceMap.unlock();
 
     qint64 ncalctime =  std::chrono::system_clock::now().time_since_epoch().count() - ncompstart;

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

@@ -16,6 +16,8 @@
 #include<image_transport/image_transport.h>
 
 #include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PointStamped.h>
+#include <geometry_msgs/TwistStamped.h>
 
 #include "autoware_msgs/Lane.h"
 
@@ -160,8 +162,10 @@ void Listenodom(const char * strdata,const unsigned int nSize,const unsigned int
     msg.twist.twist.angular.z = xodom.twist_angula().z();
 
 
-    geometry_msgs::Pose xPose = msg.pose.pose;
-    geometry_msgs::Twist xtwist = msg.twist.twist;
+    geometry_msgs::PoseStamped  xPose;
+    xPose.pose = msg.pose.pose;
+    geometry_msgs::TwistStamped xtwist;
+    xtwist.twist = msg.twist.twist;
 
     pose_pub.publish(xPose);
     twist_pub.publish(xtwist);
@@ -256,6 +260,8 @@ void ListenWayPointsMap(const char * strdata,const unsigned int nSize,const unsi
         xpoint.pose.pose.orientation.y = quat.y;
         xpoint.pose.pose.orientation.z = quat.z;
         xpoint.pose.pose.orientation.w = quat.w;
+
+        xpoint.twist.twist.linear.x = ppoint->speed();
         xlane.waypoints.push_back(xpoint);
         
 
@@ -321,8 +327,8 @@ int main(int argc, char *argv[])
                 _point_topic, 10);
 
     waypoints_pub = private_nh.advertise<autoware_msgs::Lane>(_waypoints_topic,10);
-    pose_pub = private_nh.advertise<geometry_msgs::Pose>(_pose_topic,10);
-    twist_pub = private_nh.advertise<geometry_msgs::Twist>(_twist_topic,10);
+    pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic,10);
+    twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic,10);
 	
 
 
@@ -336,6 +342,8 @@ int main(int argc, char *argv[])
     void * pliar = iv::modulecomm::RegisterRecv(_point_msgname.data(),ListenPointCloud);
     void * podom = iv::modulecomm::RegisterRecv(_odom_msgname.data(),Listenodom);
 
+    void * pway = iv::modulecomm::RegisterRecv(_waypoints_msgname.data(),ListenWayPointsMap);
+
 
 
 ros::spin();

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

@@ -237,11 +237,14 @@ bool PurePursuit::canGetCurvature(double* output_kappa)
 {
   // search next waypoint
   getNextWaypoint();
+
   if (next_waypoint_number_ == -1)
   {
     ROS_INFO("lost next waypoint");
     return false;
   }
+  
+
   // check whether curvature is valid or not
   bool is_valid_curve = false;
   for (const auto& el : current_waypoints_)
@@ -257,6 +260,8 @@ bool PurePursuit::canGetCurvature(double* output_kappa)
   {
     return false;
   }
+
+
   // if is_linear_interpolation_ is false or next waypoint is first or last
   if (!is_linear_interpolation_ || next_waypoint_number_ == 0 ||
     next_waypoint_number_ == (static_cast<int>(current_waypoints_.size() - 1)))
@@ -281,6 +286,7 @@ bool PurePursuit::canGetCurvature(double* output_kappa)
   //  next_target.x, next_target.y,next_target.z);
 
   *output_kappa = calcCurvature(next_target_position_);
+    ROS_WARN("get kappa %lf... ",*output_kappa);
   return true;
 }
 

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

@@ -106,6 +106,8 @@ void PurePursuitNode::run()
       continue;
     }
 
+    
+  
     pp_.setLookaheadDistance(computeLookaheadDistance());
     pp_.setMinimumLookaheadDistance(minimum_lookahead_distance_);
 
@@ -209,6 +211,7 @@ double PurePursuitNode::computeCommandVelocity() const
     return getSgn() * kmph2mps(const_velocity_);
   }
 
+  ROS_WARN("get velocity  %lf... ",command_linear_velocity_);
   return command_linear_velocity_;
 }
 
@@ -286,6 +289,7 @@ void PurePursuitNode::callbackFromCurrentVelocity(
 void PurePursuitNode::callbackFromWayPoints(
   const autoware_msgs::LaneConstPtr& msg)
 {
+  ROS_WARN(" TWIST X %lf",msg->waypoints.at(0).twist.twist.linear.x );
   command_linear_velocity_ =
     (!msg->waypoints.empty()) ? msg->waypoints.at(0).twist.twist.linear.x : 0;
   if (add_virtual_end_waypoints_)