change pilottoros. for advise waypoints msg.

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

@@ -35,9 +35,18 @@ static std::string _image_msgname = "picfront";
 static std::string  _odom_topic = "/odom";
 static std::string _odom_msgname = "odom";
 
+static std::string _waypoints_topic = "/final_waypoints";
+static std::string _waypoints_msgname = "waypoints";
+
+static std::string _pose_topic = "/current_pose";
+static std::string _twist_topic =  "/current_velocity";
+
 ros::Publisher  point_cloud_pub;
 image_transport::Publisher  image_pub;
 ros::Publisher odom_pub;
+ros::Publisher waypoints_pub;
+ros::Publisher pose_pub;
+ros::Publisher twist_pub;
 
 
 static iv::map::tracemap gtracemap;
@@ -154,6 +163,9 @@ void Listenodom(const char * strdata,const unsigned int nSize,const unsigned int
     geometry_msgs::Pose xPose = msg.pose.pose;
     geometry_msgs::Twist xtwist = msg.twist.twist;
 
+    pose_pub.publish(xPose);
+    twist_pub.publish(xtwist);
+
 
   odom_pub.publish(msg);
 
@@ -244,16 +256,20 @@ 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;
+        xlane.waypoints.push_back(xpoint);
         
 
     }
 
+    waypoints_pub.publish(xlane);
 
 
+/*
     gMutexTraceMap.lock();
     gtracemap.CopyFrom(xtracemap);
     gbHaveMap = true;
     gMutexTraceMap.unlock();
+*/
 }
 
 void ThreadCalcNearPoint()
@@ -296,8 +312,17 @@ int main(int argc, char *argv[])
     std::cout<<"_image_topic is "<<_image_topic<<std::endl;
 	std::cout<<"_image_msgname : "<<_image_msgname<<std::endl;
 
+    private_nh.getParam("waypoints_node",_waypoints_topic);
+    private_nh.getParam("waypoints_msgname",_waypoints_msgname);
+    std::cout<<"_waypoints_topic is "<<_waypoints_topic<<std::endl;
+	std::cout<<"_waypoints_msgname : "<<_waypoints_msgname<<std::endl;
+
     point_cloud_pub = private_nh.advertise<sensor_msgs::PointCloud2>(
                 _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);