Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support UUST2 with fixed firmware #177

Merged
merged 6 commits into from
Nov 25, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 8 additions & 3 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,16 @@ Also, the resolution of the timestamp was not enough for a high-speed motion of

So, urg\_stamped estimates sub-millisecond by the following algorithm:

- Select the algorithms based on the sensor model
- UTM: UTM-30LX-EW
- UST(UUST1): UST-\*LX, UST-\*LC with firmware version <4.0.0
- UST(UUST2-Unfixed): UST-\*LX, UST-\*LC with firmware version >=4.0.0 and <4.0.3
- UST(UUST2-Fixed): UST-\*LX, UST-\*LC with firmware version >=4.0.3
- Determine sensor internal clock state (clock offset and gain) using TM command (Triggered by 30s timer by default)
- UTM/UST(UUST1): (UTM and UST(UUST1) responds to TM command as expected in SCIP2 protocol)
- UTM/UST(UUST1)/UST(UUST2-Fixed): (These models respond to TM command as expected in SCIP2 protocol)
- 1. Observe sub-millisecond clock offset by finding increment of millisecond resolution sensor timestamp
- 2. Observe clock gain from multiple observations of the clock offset
- UST(UUST2): (UST(UUST2) responds to TM command on the next 5ms frame which breaks SCIP2's time synchronization logic)
- UST(UUST2-Unfixed): (This model responds to TM command on the next 5ms frame which breaks SCIP2's time synchronization logic)
- 1. Request TM command many times with different timing
- 2. Filter responses with large delay
- 3. Collect sensor response timings which should be synchronized to the sensor timestamp increment
Expand All @@ -29,7 +34,7 @@ So, urg\_stamped estimates sub-millisecond by the following algorithm:
- 2. Observe scan origin time and scan interval using scan timestamp jitter
- Calculate sub-millisecond scan timestamp based on the observed scan origin time and scan interval

LaserScan data is stopped during sensor internal clock estimation which takes at most ~100ms on UTM/UUST1 and ~1s on UUST2.
LaserScan data is stopped during sensor internal clock estimation which takes at most ~100ms on UTM/UST(UUST1)/UST(UUST2-Fixed) and ~1s on UST(UUST2-Unfixed).
To avoid stopping all sensors at once on multi-sensor configuration, urg\_stamped automatically adjusts the timing of sensor internal clock estimation based on the messages on urg\_stamped\_sync\_start topic.

## Usages
Expand Down
43 changes: 43 additions & 0 deletions include/urg_stamped/strings.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
/*
* Copyright 2024 The urg_stamped Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#ifndef URG_STAMPED_STRINGS_H
#define URG_STAMPED_STRINGS_H

#include <sstream>
#include <string>
#include <vector>

namespace urg_stamped
{
namespace strings
{
inline std::vector<std::string> split(const std::string& s, const char delim)
{
std::vector<std::string> out;
std::stringstream stream(s);
std::string el;
while (std::getline(stream, el, delim))
{
out.push_back(el);
}
return out;
}
}; // namespace strings
}; // namespace urg_stamped

#endif // URG_STAMPED_STRINGS_H

38 changes: 28 additions & 10 deletions src/urg_stamped.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <scip2/logger.h>

#include <urg_stamped/ros_logger.h>
#include <urg_stamped/strings.h>

#include <urg_stamped/urg_stamped.h>

Expand Down Expand Up @@ -333,19 +334,32 @@
}
if (!est_)
{
int firm_major;
int firm_major = 1;
int firm_minor = 0;
int firm_patch = 0;
{
const auto firm_it = params.find("FIRM");
if (firm_it == params.end())
{
scip2::logger::error()
<< "Could not detect sensor hardware revision. Fallback to UUST1 mode"
<< std::endl;
firm_major = 1;
}
else
{
firm_major = std::stoi(firm_it->second);
const std::vector<std::string> version = urg_stamped::strings::split(firm_it->second, '.');
if (version.size() < 3)
{
scip2::logger::error()

Check warning on line 353 in src/urg_stamped.cpp

View check run for this annotation

Codecov / codecov/patch

src/urg_stamped.cpp#L353

Added line #L353 was not covered by tests
<< "Invalid firmware version (" << firm_it->second << "). Fallback to UUST1 mode"
<< std::endl;
}
else
{
firm_major = std::stoi(version[0]);
firm_minor = std::stoi(version[1]);
firm_patch = std::stoi(version[2]);
}
}
}
std::string prod;
Expand Down Expand Up @@ -380,15 +394,15 @@
<< "Unknown sensor model. Fallback to UST mode"
<< std::endl;
}
if (firm_major >= 4)
if (firm_major == 4 && firm_minor == 0 && firm_patch < 3)
{
model = "UST (UUST2)";
model = "UST (UUST2Unfixed)";
clock.reset(new device_state_estimator::ClockEstimatorUUST2());
is_uust2_ = true;
}
else
{
model = "UST (UUST1)";
model = "UST (UUST1, UUST2Fixed)";
clock.reset(new device_state_estimator::ClockEstimatorUUST1());
}
scan.reset(new device_state_estimator::ScanEstimatorUST(clock, ideal_scan_interval_));
Expand Down Expand Up @@ -586,15 +600,13 @@
tm_try_count_ = 0;
scip2::logger::debug() << "Starting communication delay estimation" << std::endl;
delay_estim_state_ = DelayEstimState::STOPPING_SCAN;
timer_retry_tm_.stop();
timer_retry_tm_ = nh_.createTimer(
tm_command_interval_,
&UrgStampedNode::retryTM, this);
retryTM();
}

void UrgStampedNode::retryTM(const ros::TimerEvent& event)
{
bool retry = true;
timer_retry_tm_.stop();
switch (delay_estim_state_)
{
case DelayEstimState::STOPPING_SCAN:
Expand All @@ -620,10 +632,16 @@
case DelayEstimState::ESTIMATING:
scip2::logger::warn() << "Timeout occured during the time synchronization" << std::endl;
scip_->sendCommand("TM2");
retry = false;
break;
default:
retry = false;
break;
}
if (retry)
{
timer_retry_tm_ = nh_.createTimer(tm_command_interval_, &UrgStampedNode::retryTM, this, true);
}
}

void UrgStampedNode::errorCountIncrement(const std::string& status)
Expand Down
2 changes: 2 additions & 0 deletions test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,8 @@ target_link_libraries(test_device_state_estimator ${catkin_LIBRARIES} ${Boost_LI
catkin_add_gtest(test_decode src/test_decode.cpp)
target_link_libraries(test_decode ${catkin_LIBRARIES} ${Boost_LIBRARIES})

catkin_add_gtest(test_strings src/test_strings.cpp)

catkin_add_gtest(test_param src/test_param.cpp)

catkin_add_gtest(test_scip2
Expand Down
39 changes: 39 additions & 0 deletions test/src/test_strings.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
/*
* Copyright 2024 The urg_stamped Authors
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/

#include <gtest/gtest.h>

#include <string>
#include <vector>

#include <urg_stamped/strings.h>

TEST(Strings, Split)
{
const std::string in("1.23.456");
const std::vector<std::string> out = urg_stamped::strings::split(in, '.');
ASSERT_EQ(out.size(), 3);
ASSERT_EQ(out[0], std::string("1"));
ASSERT_EQ(out[1], std::string("23"));
ASSERT_EQ(out[2], std::string("456"));
}

int main(int argc, char** argv)
{
testing::InitGoogleTest(&argc, argv);

return RUN_ALL_TESTS();
}