├── README.md
├── package.xml
├── CMakeLists.txt
└── src
    └── UdpROS.cpp


/README.md:
--------------------------------------------------------------------------------
 1 | # ros_UDP
 2 | 
 3 | This is a small program used to set up UDP communication between ROS and dSpace MABX in our paper
 4 | 
 5 | "Localization and Perception for Control and Decision Making of a Low Speed Autonomous Shuttle in a Campus Pilot Deployment". SAE Technical Paper (2018): 01-1182
 6 | https://www.researchgate.net/publication/324190639_Localization_and_Perception_for_Control_and_Decision_Making_of_a_Low_Speed_Autonomous_Shuttle_in_a_Campus_Pilot_Deployment
 7 | 
 8 | ## Video:
 9 | https://www.youtube.com/watch?v=LwrdPRxHzrg
10 | 
11 | It can receive or send data on ROS. It's a project about coordination between SLAM and dSpace controller. You can change it for anything you want
12 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>udpros</name>
 4 |   <version>0.0.0</version>
 5 |   <description>The udpros package</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="365867978@qq.com">Bowen Wen</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>TODO</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <!-- <url type="website">http://wiki.ros.org/udpros</url> -->
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 | 	<author email="365867978@qq.com">Bowen Wen</author> 
29 | 
30 | 
31 |   <!-- The *_depend tags are used to specify dependencies -->
32 |   <!-- Dependencies can be catkin packages or system dependencies -->
33 |   <!-- Examples: -->
34 |   <!-- Use build_depend for packages you need at compile time: -->
35 |   <!--   <build_depend>message_generation</build_depend> -->
36 |   <!-- Use buildtool_depend for build tool packages: -->
37 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
38 |   <!-- Use run_depend for packages you need at runtime: -->
39 |   <!--   <run_depend>message_runtime</run_depend> -->
40 |   <!-- Use test_depend for packages you need only for testing: -->
41 |   <!--   <test_depend>gtest</test_depend> -->
42 |   <buildtool_depend>catkin</buildtool_depend>
43 |   <build_depend>geometry_msgs</build_depend>
44 |   <build_depend>roscpp</build_depend>
45 |   <run_depend>geometry_msgs</run_depend>
46 |   <run_depend>roscpp</run_depend>
47 | 
48 | 
49 |   <!-- The export tag contains other, unspecified, tags -->
50 |   <export>
51 |     <!-- Other tools can request additional information be placed here -->
52 | 
53 |   </export>
54 | </package>
55 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(udpros)
  3 | 
  4 | ## Find catkin macros and libraries
  5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  6 | ## is used, also find other catkin packages
  7 | find_package(catkin REQUIRED COMPONENTS
  8 |   geometry_msgs
  9 |   roscpp
 10 | )
 11 | 
 12 | ## System dependencies are found with CMake's conventions
 13 | # find_package(Boost REQUIRED COMPONENTS system)
 14 | 
 15 | 
 16 | ## Uncomment this if the package has a setup.py. This macro ensures
 17 | ## modules and global scripts declared therein get installed
 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
 19 | # catkin_python_setup()
 20 | 
 21 | ################################################
 22 | ## Declare ROS messages, services and actions ##
 23 | ################################################
 24 | 
 25 | ## To declare and build messages, services or actions from within this
 26 | ## package, follow these steps:
 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in
 28 | ##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
 29 | ## * In the file package.xml:
 30 | ##   * add a build_depend tag for "message_generation"
 31 | ##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
 32 | ##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
 33 | ##     but can be declared for certainty nonetheless:
 34 | ##     * add a run_depend tag for "message_runtime"
 35 | ## * In this file (CMakeLists.txt):
 36 | ##   * add "message_generation" and every package in MSG_DEP_SET to
 37 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 38 | ##   * add "message_runtime" and every package in MSG_DEP_SET to
 39 | ##     catkin_package(CATKIN_DEPENDS ...)
 40 | ##   * uncomment the add_*_files sections below as needed
 41 | ##     and list every .msg/.srv/.action file to be processed
 42 | ##   * uncomment the generate_messages entry below
 43 | ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 44 | 
 45 | ## Generate messages in the 'msg' folder
 46 | # add_message_files(
 47 | #   FILES
 48 | #   Message1.msg
 49 | #   Message2.msg
 50 | # )
 51 | 
 52 | ## Generate services in the 'srv' folder
 53 | # add_service_files(
 54 | #   FILES
 55 | #   Service1.srv
 56 | #   Service2.srv
 57 | # )
 58 | 
 59 | ## Generate actions in the 'action' folder
 60 | # add_action_files(
 61 | #   FILES
 62 | #   Action1.action
 63 | #   Action2.action
 64 | # )
 65 | 
 66 | ## Generate added messages and services with any dependencies listed here
 67 | # generate_messages(
 68 | #   DEPENDENCIES
 69 | #   geometry_msgs
 70 | # )
 71 | 
 72 | ################################################
 73 | ## Declare ROS dynamic reconfigure parameters ##
 74 | ################################################
 75 | 
 76 | ## To declare and build dynamic reconfigure parameters within this
 77 | ## package, follow these steps:
 78 | ## * In the file package.xml:
 79 | ##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
 80 | ## * In this file (CMakeLists.txt):
 81 | ##   * add "dynamic_reconfigure" to
 82 | ##     find_package(catkin REQUIRED COMPONENTS ...)
 83 | ##   * uncomment the "generate_dynamic_reconfigure_options" section below
 84 | ##     and list every .cfg file to be processed
 85 | 
 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder
 87 | # generate_dynamic_reconfigure_options(
 88 | #   cfg/DynReconf1.cfg
 89 | #   cfg/DynReconf2.cfg
 90 | # )
 91 | 
 92 | ###################################
 93 | ## catkin specific configuration ##
 94 | ###################################
 95 | ## The catkin_package macro generates cmake config files for your package
 96 | ## Declare things to be passed to dependent projects
 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files
 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need
 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
100 | ## DEPENDS: system dependencies of this project that dependent projects also need
101 | catkin_package(
102 |   INCLUDE_DIRS include
103 |   LIBRARIES udpros
104 |   CATKIN_DEPENDS  geometry_msgs  roscpp
105 |   DEPENDS system_lib
106 | )
107 | 
108 | ###########
109 | ## Build ##
110 | ###########
111 | 
112 | ## Specify additional locations of header files
113 | ## Your package locations should be listed before other locations
114 | # include_directories(include)
115 | include_directories(
116 |   ${catkin_INCLUDE_DIRS}
117 | 	/home/wbw/catkin_ws/
118 | )
119 | 
120 | ## Declare a C++ library
121 | # add_library(udpros
122 | #   src/${PROJECT_NAME}/udpros.cpp
123 | # )
124 | 
125 | ## Add cmake target dependencies of the library
126 | ## as an example, code may need to be generated before libraries
127 | ## either from message generation or dynamic reconfigure
128 | # add_dependencies(udpros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
129 | 
130 | ## Declare a C++ executable
131 |  add_executable(udpros src/UdpROS.cpp)
132 | 
133 | ## Add cmake target dependencies of the executable
134 | ## same as for the library above
135 |  add_dependencies(udpros ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
136 | 
137 | ## Specify libraries to link a library or executable target against
138 |  target_link_libraries(udpros
139 |    ${catkin_LIBRARIES}
140 |  )
141 | 
142 | #############
143 | ## Install ##
144 | #############
145 | 
146 | # all install targets should use catkin DESTINATION variables
147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
148 | 
149 | ## Mark executable scripts (Python etc.) for installation
150 | ## in contrast to setup.py, you can choose the destination
151 | # install(PROGRAMS
152 | #   scripts/my_python_script
153 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
154 | # )
155 | 
156 | ## Mark executables and/or libraries for installation
157 | # install(TARGETS udpros udpros_node
158 | #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
159 | #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
160 | #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
161 | # )
162 | 
163 | ## Mark cpp header files for installation
164 | # install(DIRECTORY include/${PROJECT_NAME}/
165 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
166 | #   FILES_MATCHING PATTERN "*.h"
167 | #   PATTERN ".svn" EXCLUDE
168 | # )
169 | 
170 | ## Mark other files for installation (e.g. launch and bag files, etc.)
171 | # install(FILES
172 | #   # myfile1
173 | #   # myfile2
174 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
175 | # )
176 | 
177 | #############
178 | ## Testing ##
179 | #############
180 | 
181 | ## Add gtest based cpp test target and link libraries
182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_udpros.cpp)
183 | # if(TARGET ${PROJECT_NAME}-test)
184 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
185 | # endif()
186 | 
187 | ## Add folders to be run by python nosetests
188 | # catkin_add_nosetests(test)
189 | 


--------------------------------------------------------------------------------
/src/UdpROS.cpp:
--------------------------------------------------------------------------------
  1 | //=================================================================================================
  2 | // Copyright (c) 2018, Bowen Wen, Ohio State University
  3 | // All rights reserved.
  4 | 
  5 | // Redistribution and use in source and binary forms, with or without
  6 | // modification, are permitted provided that the following conditions are met:
  7 | //     * Redistributions of source code must retain the above copyright
  8 | //       notice, this list of conditions and the following disclaimer.
  9 | //     * Redistributions in binary form must reproduce the above copyright
 10 | //       notice, this list of conditions and the following disclaimer in the
 11 | //       documentation and/or other materials provided with the distribution.
 12 | 
 13 | 
 14 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 15 | // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 16 | // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 17 | // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
 18 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 19 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 20 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 21 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 22 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 23 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 24 | //=================================================================================================
 25 | 
 26 | #include <boost/asio.hpp>
 27 | #include <boost/array.hpp>
 28 | #include <ros/ros.h>
 29 | #include <geometry_msgs/PoseWithCovarianceStamped.h>
 30 | #include <nav_msgs/Odometry.h>
 31 | #include <deque>
 32 | #include <cmath>
 33 | #include <fstream>
 34 | #include "ll2utm.h"
 35 | 
 36 | using boost::asio::ip::udp;
 37 | 
 38 | 
 39 | class Quaterniond
 40 | {
 41 | public:
 42 |     double w,x,y,z;
 43 |     Quaterniond(double x1,double x2,double x3,double x4)
 44 |     {
 45 |         w=x1;
 46 |         x=x2;
 47 |         y=x3;
 48 |         z=x4;
 49 |     }
 50 |     Quaterniond() {}
 51 | 
 52 | };
 53 | 
 54 | 
 55 | inline static Quaterniond ToQuaternion(double pitch, double roll, double yaw) // Here, yaw is heading angle in GPS
 56 | {
 57 |     Quaterniond q;
 58 |     pitch=0.0;
 59 |     roll=0.0;
 60 |     double t0 = std::cos(yaw * 0.5);
 61 |     double t1 = std::sin(yaw * 0.5);
 62 |     double t2 = std::cos(roll * 0.5);
 63 |     double t3 = std::sin(roll * 0.5);
 64 |     double t4 = std::cos(pitch * 0.5);
 65 |     double t5 = std::sin(pitch * 0.5);
 66 | 
 67 |     q.w = t0 * t2 * t4 + t1 * t3 * t5;
 68 |     q.x = t0 * t3 * t4 - t1 * t2 * t5;
 69 |     q.y = t0 * t2 * t5 + t1 * t3 * t4;
 70 |     q.z = t1 * t2 * t4 - t0 * t3 * t5;
 71 |     return q;
 72 | }
 73 | 
 74 | inline double GetYaw(Quaterniond q) // from quaternion form
 75 | {
 76 |     return std::atan2(2*(q.w*q.z+q.y*q.x),1-2*(std::pow(q.y,2)+std::pow(q.z,2)));
 77 | }
 78 | 
 79 | 
 80 | class Udpros
 81 | {
 82 | private:
 83 |     boost::asio::io_service io_service;
 84 |     udp::endpoint receiver_endpoint;
 85 |     udp::endpoint sender_endpoint;
 86 |     udp::socket socket;
 87 |     boost::system::error_code error;
 88 |     boost::system::error_code ignored_error;
 89 |     std::vector<int> message;
 90 |     int recv_buf[25];
 91 | 
 92 | 
 93 |     ros::Subscriber loc_sub;
 94 |     ros::Subscriber dist_sub;
 95 |     ros::Publisher gps_pub;
 96 |     nav_msgs::Odometry location;
 97 | 
 98 |     nav_msgs::Odometry gps_pub_;
 99 | 
100 |     bool ros_listen_, ros_talk_;
101 |     std::string pose_topic_,dist_topic_;
102 | 
103 |     double initial_x,initial_y,initial_heading;
104 |     bool isfirst;
105 | 
106 |     std::ofstream myfile;
107 | 
108 | public:
109 |     Udpros(ros::NodeHandle nh, ros::NodeHandle Private_nh);
110 |     ~Udpros(){myfile.close();}
111 |     void ProcessData(const nav_msgs::Odometry::ConstPtr &location);
112 |     void ProcessGPS();
113 |     void FillBuffer(double val);
114 |     double GetBuffer(int * recv_buf, int n);
115 |     void PoseROS2World(double (&p)[2]);
116 |     void PoseWorld2ROS(double (&pose)[2]);
117 | 
118 | 
119 | };
120 | 
121 | Udpros::Udpros(ros::NodeHandle nh, ros::NodeHandle Private_nh):socket(io_service, udp::endpoint(udp::v4(), 5555)),
122 |     receiver_endpoint(boost::asio::ip::address::from_string("192.168.0.89"), 5002)
123 | {
124 | //    initial_heading=-M_PI/2;
125 |     myfile.open("/home/wbw/log.txt");
126 |     isfirst=true;
127 |     Private_nh.param("ros_listen",ros_listen_,false);
128 |     Private_nh.param("ros_talk",ros_talk_,true);
129 |     Private_nh.param<std::string>("pose_topic",pose_topic_,"scanmatch_odom");
130 |     Private_nh.param<std::string>("dist_topic",dist_topic_,"mindistance");
131 |     Private_nh.param<double>("GPSh",initial_heading,-90); // [deg]
132 | 
133 |     gps_pub=nh.advertise<nav_msgs::Odometry>("odometry",100);
134 |     loc_sub=nh.subscribe(pose_topic_,10,&Udpros::ProcessData,this,ros::TransportHints().tcpNoDelay(true));
135 |     std::cout<<"initial heading="<<initial_heading<<std::endl;
136 | 
137 |     if (error && error != boost::asio::error::message_size)
138 |         throw boost::system::system_error(error);
139 | 
140 | }
141 | 
142 | void Udpros::ProcessData(const nav_msgs::Odometry::ConstPtr &location)
143 | {
144 | 
145 |     double yaw;
146 |     message.clear();
147 |     double pose[2];
148 | 
149 |     pose[0]=location->pose.pose.position.x;  // message:{x,y,yaw}
150 |     pose[1]=location->pose.pose.position.y;
151 | //    std::cout<<"raw:"<<pose[0]<<"       "<<pose[1]<<std::endl<<std::endl;
152 | 
153 | 
154 | 
155 |     PoseROS2World(pose);
156 | 
157 | //    std::cout<<"new:"<<pose[0]<<"       "<<pose[1]<<std::endl<<std::endl;
158 |     FillBuffer(pose[0]);
159 |     FillBuffer(pose[1]);
160 | 
161 |     Quaterniond q;
162 |     q.w=location->pose.pose.orientation.w;
163 |     q.x=location->pose.pose.orientation.x;
164 |     q.y=location->pose.pose.orientation.y;
165 |     q.z=location->pose.pose.orientation.z;
166 | 
167 |     yaw=GetYaw(q);
168 | 
169 |     yaw=yaw*180/M_PI;
170 |     yaw=(initial_heading-yaw); // to world coordinate [deg]
171 |     if (yaw<-180)
172 |     {
173 |         yaw=yaw+360;
174 |     }
175 |     if (yaw>180)
176 |     {
177 |         yaw=yaw-360;
178 |     }
179 | 
180 |     FillBuffer(yaw);
181 | 
182 |     std::vector<int>::iterator ite;
183 | //    ite=message.begin();
184 | //    for (ite;ite!=message.end();ite++)
185 | //    {
186 | //        std::cout<<*ite<<" ";
187 | //    }
188 | //    std::cout<<std::endl;
189 | 
190 |     std::cout<<"sending: x="<<pose[0]<<",y="<<pose[1]<<",yaw="<<yaw<<std::endl;
191 | 
192 |     socket.send_to(boost::asio::buffer(message), receiver_endpoint, 0, ignored_error);  //return The number of bytes sent.
193 | 
194 |     size_t len = socket.receive_from(boost::asio::buffer(recv_buf), sender_endpoint);
195 | 
196 | 
197 |     for (int k=0;k<12;k++)
198 |     {
199 |         myfile<<recv_buf[k]<<" ";
200 |     }
201 |      myfile<<"\n";
202 |     ProcessGPS(); // get gps and publish as topic
203 | 
204 | }
205 | 
206 | void Udpros::ProcessGPS()
207 | {
208 | //    if (isfirst==true)
209 | //    {
210 | //        initial_x=GetBuffer(recv_buf,0);
211 | //        initial_y=GetBuffer(recv_buf,3);
212 | ////        double tempx,tempy;
213 | //        LLtoUTM(initial_x,initial_y,initial_x,initial_y);
214 | //        isfirst=false;
215 | //    }
216 |     std::cout<<"initial pose="<<initial_x<<","<<initial_y<<std::endl;
217 | //    gps_pub_.header.stamp=location->header.stamp; // borrow location's stamp
218 |     gps_pub_.header.stamp=ros::Time::now();
219 | 
220 |     gps_pub_.header.frame_id="odom";
221 |     gps_pub_.child_frame_id="base_link";
222 |     gps_pub_.pose.pose.position.z=0;
223 | 
224 |     //recv_buf should be {x,y,yaw,vE,vN,aE,aN}
225 | 
226 | 
227 | 
228 | 
229 |     double pose[2],vel[2],yaw,yaw_rate;
230 |     pose[0]=GetBuffer(recv_buf,0);
231 | 
232 | //    std::cout<<pose[0]<<"++++++++++++++++++++++++++++++++"<<std::endl;
233 | 
234 | 
235 |     pose[1]=GetBuffer(recv_buf,3);
236 | //    std::cout<<pose[1]<<"++++++++++++++++++++++++++++++++"<<std::endl;
237 | //    LLtoUTM(pose[0],pose[1],pose[0],pose[1]);
238 | 
239 | //    std::cout<<"by conversion: "<<pose[0]<<std::endl;
240 | 
241 | //    pose[0]-=initial_x;
242 | //    pose[1]-=initial_y;
243 |     std::cout<<"from gps,x="<<pose[0]<<", y="<<pose[1]<<std::endl;
244 |     yaw=GetBuffer(recv_buf,6);  // [deg]
245 |     yaw=yaw*M_PI/180.0;
246 | 
247 |     vel[0]=GetBuffer(recv_buf,9);
248 |     vel[1]=GetBuffer(recv_buf,12);
249 |     yaw_rate=GetBuffer(recv_buf,15);
250 | 
251 | 
252 | 
253 |     Quaterniond q=ToQuaternion(0.0,0.0,yaw);
254 | //    PoseWorld2ROS(pose);
255 |     gps_pub_.pose.pose.position.x=pose[0];
256 |     gps_pub_.pose.pose.position.y=pose[1];
257 |     gps_pub_.pose.pose.position.z=0.0;
258 | 
259 |     gps_pub_.twist.twist.linear.x=vel[0];
260 |     gps_pub_.twist.twist.linear.y=vel[1];
261 |     gps_pub_.twist.twist.angular.z=yaw_rate;
262 | 
263 | //    gps_pub_.twist.twist.angular.z=     // yaw angular speed
264 | 
265 | //    std::cout<<"gps: x:"<<gps_pub_.pose.pose.position.x<<" y:"<<gps_pub_.pose.pose.position.y<<" vx:"<<gps_pub_.twist.twist.linear.x<<" vy:"
266 | //            <<gps_pub_.twist.twist.linear.y<<" ax:"<<" ay:"<<" yaw"<<std::endl;
267 | 
268 | 
269 | 
270 |     gps_pub_.pose.pose.orientation.w=q.w;
271 |     gps_pub_.pose.pose.orientation.x=q.x;
272 |     gps_pub_.pose.pose.orientation.y=q.y;
273 |     gps_pub_.pose.pose.orientation.z=q.z;
274 | 
275 |     std::cout<<"gps heading="<<yaw<<" "<<vel[0]<<" "<<vel[1]<<" "<<yaw_rate<<std::endl;
276 |     gps_pub.publish(gps_pub_);
277 | }
278 | 
279 | void Udpros::PoseROS2World(double (&p)[2])
280 | {
281 | 
282 | 
283 |     double theta=initial_heading*M_PI/180-M_PI/2;
284 |     double x,y;
285 | 
286 |     x=std::cos(theta)*p[0]+std::sin(theta)*p[1];
287 |     y=-std::sin(theta)*p[0]+std::cos(theta)*p[1];
288 |     p[0]=x;
289 |     p[1]=y;
290 | }
291 | 
292 | void Udpros::PoseWorld2ROS(double (&pose)[2])
293 | {
294 |     double theta=M_PI/2-initial_heading*M_PI/180;
295 |     double pose1[2];
296 |     pose1[0]=std::cos(theta)*pose[0]+std::sin(theta)*pose[1];
297 |     pose1[1]=-std::sin(theta)*pose[0]+std::cos(theta)*pose[1];
298 |     pose[0]=pose1[0];
299 |     pose[1]=pose1[1];
300 | }
301 | 
302 | 
303 | 
304 | void Udpros::FillBuffer(double val)
305 | {
306 | 
307 |     if (val>=0)
308 |     {
309 |         message.push_back(0);
310 |     }
311 |     else
312 |     {
313 |         message.push_back(1);
314 |         val=-val;
315 |     }
316 | 
317 |     message.push_back((int)val);
318 |     std::vector<int>::iterator i=message.end();
319 |     message.push_back((int)((val-*(i-1))*1000000));
320 | }
321 | 
322 | double Udpros::GetBuffer(int * recv_buf,int n)
323 | {
324 | //    std::cout<<"..................."<<((double)recv_buf[n+2]/1000000.0)<<std::endl;
325 |     if (recv_buf[n]==0) // positive
326 |     {
327 | //        printf("%f\n",(double)recv_buf[n+1]+((double)recv_buf[n+2]/1000000.0));
328 | //        std::cout<<"..................."<<((double)recv_buf[n+2]/1000000.0)<<"//"<<(double)recv_buf[n+1]<<"//"<<(double)recv_buf[n+1]+((double)recv_buf[n+2]/1000000.0)<<std::endl;
329 |         return (double)recv_buf[n+1]+((double)recv_buf[n+2]/1000000.0);
330 |     }
331 |     else // negatvie
332 |     {
333 |         return -(double)recv_buf[n+1]-((double)recv_buf[n+2]/1000000.0);
334 |     }
335 | }
336 | 
337 | int main(int argc, char ** argv)
338 | {
339 |     ros::init(argc,argv,"Udpros");
340 |     ros::NodeHandle nh;
341 |     ros::NodeHandle priv_nh("~");
342 |     Udpros udpros(nh,priv_nh);
343 |     ros::Rate r(100);
344 |     while(ros::ok())
345 |     {
346 |         ros::spinOnce();
347 |         r.sleep();
348 |     }
349 |     return 0;
350 | }
351 | 


--------------------------------------------------------------------------------