sync_timing.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2024 The urg_stamped Authors
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <cmath>
18 #include <iostream>
19 #include <map>
20 #include <memory>
21 #include <sstream>
22 #include <string>
23 #include <thread>
24 #include <vector>
25 
26 #include <boost/asio/ip/tcp.hpp>
27 
28 #include <ros/ros.h>
29 #include <sensor_msgs/LaserScan.h>
30 #include <std_msgs/Header.h>
31 #include <urg_stamped/Status.h>
32 
33 #include <ros/network.h>
34 #include <ros/xmlrpc_manager.h>
35 #include <xmlrpcpp/XmlRpc.h>
36 
37 #include <urg_sim/urg_sim.h>
38 #include <e2e_utils.h>
39 
40 #include <gtest/gtest.h>
41 
42 class SyncTiming : public ::testing::Test
43 {
44 public:
46  : nh_("")
47  {
48  sub_sync_start_ = nh_.subscribe("urg_stamped_sync_start", 100, &SyncTiming::cbSyncStart, this);
49  }
50 
51  void TearDown()
52  {
53  for (int i = 0; i < 2; i++)
54  {
55  if (sim_[i])
56  {
57  sim_[i]->kill();
58  th_sim_[i].join();
59  }
60  }
61  }
62 
63 protected:
66 
68  std::thread th_sim_[2];
69  std::map<std::string, ros::Time> sync_start_time_;
70 
72  {
73  }
74 
76  {
77  sync_start_time_[msg->frame_id] = msg->stamp;
78  }
79 
80  void spinFor(const ros::Duration& timeout)
81  {
82  const ros::Time deadline = ros::Time::now() + timeout;
83  ros::Rate wait(10);
84  while (ros::Time::now() < deadline)
85  {
86  wait.sleep();
87  ros::spinOnce();
88  ASSERT_TRUE(ros::ok());
89  }
90  }
91 
93  {
94  sim_[id] = new urg_sim::URGSimulator(
95  boost::asio::ip::tcp::endpoint(
96  boost::asio::ip::tcp::v4(),
97  0),
98  params,
99  std::bind(&SyncTiming::cbRawScanData, this, std::placeholders::_1));
100  th_sim_[id] = std::thread(std::bind(&urg_sim::URGSimulator::spin, sim_[id]));
101  ros::Duration(0.1).sleep(); // Wait boot
102  }
103 
105  {
106  nh_.setParam("/urg_stamped0/ip_port", sim_[0]->getLocalEndpoint().port());
107  nh_.setParam("/urg_stamped1/ip_port", sim_[1]->getLocalEndpoint().port());
108 
109  // Shutdown urg_stamped to initialize internal state and reload parameters
110  if (!shutdownNode("urg_stamped0") || !shutdownNode("urg_stamped1"))
111  {
112  ros::Duration(1).sleep(); // Retry
113  ASSERT_TRUE(shutdownNode("urg_stamped0"));
114  ASSERT_TRUE(shutdownNode("urg_stamped1"));
115  }
116  }
117 };
118 
119 TEST_F(SyncTiming, NoConcurrentSync)
120 {
122  {
124  .boot_duration = 0.01,
125  .comm_delay_base = 0.0005,
126  .comm_delay_sigma = 0.0005,
127  .scan_interval = 0.025,
128  .clock_rate = 1.0,
129  .hex_ii_timestamp = false,
130  .angle_resolution = 1440,
131  .angle_min = 0,
132  .angle_max = 1080,
133  .angle_front = 540,
134  };
135 
136  ASSERT_NO_FATAL_FAILURE(startSimulator(0, params));
137  ASSERT_NO_FATAL_FAILURE(startSimulator(1, params));
138  ros::Duration(1).sleep(); // Wait startup
139 
140  ASSERT_NO_FATAL_FAILURE(startUrgStamped());
141  ros::Duration(1).sleep(); // Wait node respawn
142 
143  ASSERT_NO_FATAL_FAILURE(spinFor(ros::Duration(5)));
144 
145  ASSERT_TRUE(sync_start_time_["laser0"].isValid());
146  ASSERT_TRUE(sync_start_time_["laser1"].isValid());
147 
148  // Base interval is 1s
149  const double diff = std::remainder(
150  (sync_start_time_["laser0"] - sync_start_time_["laser1"]).toSec(), 1.0);
151 
152  // Sync takes more than 0.1s on this configuration,
153  // so the timings must have difference larger than 0.1s
154  ASSERT_GT(std::abs(diff), 0.1);
155 }
156 
157 int main(int argc, char** argv)
158 {
159  testing::InitGoogleTest(&argc, argv);
160  ros::init(argc, argv, "urg_stamped_e2e");
161  ros::NodeHandle nh; // workaround to keep the test node during the process life time
162 
163  return RUN_ALL_TESTS();
164 }
165 
SyncTiming::cbRawScanData
void cbRawScanData(const urg_sim::RawScanData::Ptr data)
Definition: sync_timing.cpp:71
ros::NodeHandle::setParam
void setParam(const std::string &key, bool b) const
urg_sim::URGSimulator::Params
Definition: urg_sim.h:68
TEST_F
TEST_F(SyncTiming, NoConcurrentSync)
Definition: sync_timing.cpp:119
urg_sim::URGSimulator
Definition: urg_sim.h:59
boost::shared_ptr
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
urg_sim::URGSimulator::kill
void kill()
Definition: urg_sim.cpp:825
SyncTiming::startSimulator
void startSimulator(const int id, const urg_sim::URGSimulator::Params &params)
Definition: sync_timing.cpp:92
ros.h
SyncTiming::nh_
ros::NodeHandle nh_
Definition: sync_timing.cpp:64
SyncTiming::sync_start_time_
std::map< std::string, ros::Time > sync_start_time_
Definition: sync_timing.cpp:69
ros::spinOnce
ROSCPP_DECL void spinOnce()
SyncTiming::SyncTiming
SyncTiming()
Definition: sync_timing.cpp:45
network.h
urg_sim::URGSimulator::Model::UTM
@ UTM
urg_sim::RawScanData::Ptr
std::shared_ptr< RawScanData > Ptr
Definition: urg_sim.h:47
ros::ok
ROSCPP_DECL bool ok()
e2e_utils.h
xmlrpc_manager.h
SyncTiming::TearDown
void TearDown()
Definition: sync_timing.cpp:51
SyncTiming
Definition: sync_timing.cpp:42
params
std::vector< urg_sim::URGSimulator::Params > params({ { .model=urg_sim::URGSimulator::Model::UTM,.boot_duration=0.01,.comm_delay_base=0.0002,.comm_delay_sigma=0.0004,.scan_interval=0.02505,.clock_rate=1.0,.hex_ii_timestamp=false,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, { .model=urg_sim::URGSimulator::Model::UTM,.boot_duration=0.01,.comm_delay_base=0.0002,.comm_delay_sigma=0.0004,.scan_interval=0.02505,.clock_rate=1.001,.hex_ii_timestamp=false,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, { .model=urg_sim::URGSimulator::Model::UTM,.boot_duration=0.01,.comm_delay_base=0.0002,.comm_delay_sigma=0.0004,.scan_interval=0.02505,.clock_rate=0.999,.hex_ii_timestamp=false,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, { .model=urg_sim::URGSimulator::Model::UST_UUST1,.boot_duration=0.01,.comm_delay_base=0.0005,.comm_delay_sigma=0.0005,.scan_interval=0.02505,.clock_rate=1.0,.hex_ii_timestamp=true,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, { .model=urg_sim::URGSimulator::Model::UST_UUST1,.boot_duration=0.01,.comm_delay_base=0.0005,.comm_delay_sigma=0.0005,.scan_interval=0.02505,.clock_rate=1.001,.hex_ii_timestamp=true,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, { .model=urg_sim::URGSimulator::Model::UST_UUST1,.boot_duration=0.01,.comm_delay_base=0.0005,.comm_delay_sigma=0.0005,.scan_interval=0.02505,.clock_rate=0.999,.hex_ii_timestamp=true,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, { .model=urg_sim::URGSimulator::Model::UST_UUST2,.boot_duration=0.01,.comm_delay_base=0.0002,.comm_delay_sigma=0.0002,.scan_interval=0.02505,.clock_rate=1.0,.hex_ii_timestamp=true,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, { .model=urg_sim::URGSimulator::Model::UST_UUST2,.boot_duration=0.01,.comm_delay_base=0.0002,.comm_delay_sigma=0.0002,.scan_interval=0.02505,.clock_rate=1.001,.hex_ii_timestamp=true,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, { .model=urg_sim::URGSimulator::Model::UST_UUST2,.boot_duration=0.01,.comm_delay_base=0.0002,.comm_delay_sigma=0.0002,.scan_interval=0.02505,.clock_rate=0.999,.hex_ii_timestamp=true,.angle_resolution=1440,.angle_min=0,.angle_max=1080,.angle_front=540, }, })
XmlRpc.h
SyncTiming::cbSyncStart
void cbSyncStart(const std_msgs::Header::ConstPtr &msg)
Definition: sync_timing.cpp:75
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
SyncTiming::th_sim_
std::thread th_sim_[2]
Definition: sync_timing.cpp:68
ros::Rate::sleep
bool sleep()
SyncTiming::sub_sync_start_
ros::Subscriber sub_sync_start_
Definition: sync_timing.cpp:65
urg_sim::URGSimulator::spin
void spin()
Definition: urg_sim.cpp:779
ros::Time
shutdownNode
bool shutdownNode(const std::string &name)
Definition: e2e_utils.h:30
urg_sim.h
SyncTiming::startUrgStamped
void startUrgStamped()
Definition: sync_timing.cpp:104
main
int main(int argc, char **argv)
Definition: sync_timing.cpp:157
SyncTiming::sim_
urg_sim::URGSimulator * sim_[2]
Definition: sync_timing.cpp:67
ros::Rate
SyncTiming::spinFor
void spinFor(const ros::Duration &timeout)
Definition: sync_timing.cpp:80
ros::Duration::sleep
bool sleep() const
ros::Duration
ros::NodeHandle
ros::Subscriber
ros::Time::now
static Time now()


urg_stamped
Author(s): Atsushi Watanabe
autogenerated on Wed Dec 18 2024 03:10:57