HiepLM/pnkx_nav_core
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update init

Browse Source
This commit is contained in:
HiepLM
2025-12-24 14:29:58 +07:00
parent f0225ae5e3
commit 10379d5b16
107 changed files with 5532 additions and 767 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/Libraries/common_msgs

Submodule src/Libraries/common_msgs updated: 6bac684298...e7dc4031c6

2
src/Libraries/costmap_2d

Submodule src/Libraries/costmap_2d updated: fdfba18bde...b3be5da393

2
src/Libraries/data_convert

Submodule src/Libraries/data_convert updated: 03a8fa3a80...8b22de38ac

18
src/Libraries/nav_2d_utils/CMakeLists.txt
View File

@@ -32,8 +32,7 @@ target_link_libraries(conversions
nav_msgs
nav_grid
nav_core2
robot::node_handle
robot::console
robot_cpp
PRIVATE
console_bridge::console_bridge
Boost::system
@@ -50,8 +49,7 @@ target_link_libraries(path_ops
PUBLIC
nav_2d_msgs
geometry_msgs
robot::node_handle
robot::console
robot_cpp
)
add_library(polygons src/polygons.cpp src/footprint.cpp)
@@ -64,8 +62,7 @@ target_link_libraries(polygons
PUBLIC
nav_2d_msgs
geometry_msgs
robot::node_handle
robot::console
robot_cpp
PRIVATE
${xmlrpcpp_LIBRARIES}
)
@@ -87,8 +84,7 @@ target_link_libraries(bounds
PUBLIC
nav_grid
nav_core2
robot::node_handle
robot::console
robot_cpp
)
add_library(tf_help src/tf_help.cpp)
@@ -103,8 +99,7 @@ target_link_libraries(tf_help
geometry_msgs
nav_core2
tf3
robot::node_handle
robot::console
robot_cpp
)
# Create an INTERFACE library that represents all nav_2d_utils libraries
@@ -121,8 +116,7 @@ target_link_libraries(nav_2d_utils
polygons
bounds
tf_help
robot::node_handle
robot::console
robot_cpp
)
# Install header files

4
src/Libraries/nav_2d_utils/doc/Conversions.md
View File

@@ -17,7 +17,7 @@
| to `nav_2d_msgs` | from `nav_2d_msgs` |
| -- | -- |
| `Pose2DStamped poseStampedToPose2D(geometry_msgs::PoseStamped)` | `geometry_msgs::PoseStamped pose2DToPoseStamped(Pose2DStamped)`
||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, ros::Time)`|
||`geometry_msgs::PoseStamped pose2DToPoseStamped(geometry_msgs::Pose2D, std::string, robot::Time)`|
||`geometry_msgs::Pose pose2DToPose(geometry_msgs::Pose2D)`|
| `Pose2DStamped stampedPoseToPose2D(tf::Stamped<tf::Pose>)` | |
@@ -25,7 +25,7 @@
| to `nav_2d_msgs` | from `nav_2d_msgs` |
| -- | -- |
| `Path2D pathToPath(nav_msgs::Path)` |`nav_msgs::Path pathToPath(Path2D)`
| `Path2D posesToPath2D(std::vector<geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<geometry_msgs::Pose2D>, std::string, ros::Time)`
| `Path2D posesToPath2D(std::vector<geometry_msgs::PoseStamped>)` | `nav_msgs::Path poses2DToPath(std::vector<geometry_msgs::Pose2D>, std::string, robot::Time)`
Also, `nav_msgs::Path posesToPath(std::vector<geometry_msgs::PoseStamped>)`

20
src/Libraries/nav_2d_utils/include/nav_2d_utils/parameters.h
View File

@@ -35,7 +35,10 @@
#ifndef NAV_2D_UTILS_PARAMETERS_H
#define NAV_2D_UTILS_PARAMETERS_H
#include <robot/node_handle.h>
#include <robot/console.h>
#include <string>
#include <iostream>
namespace nav_2d_utils
{
@@ -55,12 +58,13 @@ template<class param_t>
param_t searchAndGetParam(const robot::NodeHandle& nh, const std::string& param_name, const param_t& default_value)
{
std::string resolved_name;
// if (nh.searchParam(param_name, resolved_name))
// {
// param_t value = default_value;
// nh.param(resolved_name, value, default_value);
// return value;
// }
if (nh.searchParam(param_name, resolved_name))
{
param_t value = default_value;
robot::NodeHandle nh_private = robot::NodeHandle("~");
nh_private.param(resolved_name, value, default_value);
return value;
}
return default_value;
}
@@ -84,7 +88,7 @@ param_t loadParameterWithDeprecation(const robot::NodeHandle& nh, const std::str
}
if (nh.hasParam(old_name))
{
std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
nh.getParam(old_name, value, default_value);
return value;
}
@@ -103,7 +107,7 @@ void moveDeprecatedParameter(const robot::NodeHandle& nh, const std::string curr
if (!nh.hasParam(old_name)) return;
param_t value;
std::cout << "Parameter " << old_name << " is deprecated. Please use the name " << current_name << " instead." << std::endl;
robot::log_info_at(__FILE__, __LINE__, "Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
value = nh.param<param_t>(old_name);
nh.setParam(current_name, value);
}

4
src/Libraries/nav_2d_utils/include/nav_2d_utils/plugin_mux.h
View File

@@ -205,8 +205,8 @@ protected:
std::string current_plugin_;
// ROS Interface
ros::ServiceServer switch_plugin_srv_;
ros::Publisher current_plugin_pub_;
robot::ServiceServer switch_plugin_srv_;
robot::Publisher current_plugin_pub_;
robot::NodeHandle private_nh_;
std::string ros_name_;

4
src/Libraries/nav_2d_utils/src/conversions.cpp
View File

@@ -128,7 +128,7 @@ geometry_msgs::PoseStamped pose2DToPoseStamped(const nav_2d_msgs::Pose2DStamped&
}
// geometry_msgs::PoseStamped pose2DToPoseStamped(const geometry_msgs::Pose2D& pose2d,
// const std::string& frame, const ros::Time& stamp)
// const std::string& frame, const robot::Time& stamp)
// {
// geometry_msgs::PoseStamped pose;
// pose.header.frame_id = frame;
@@ -189,7 +189,7 @@ nav_2d_msgs::Path2D posesToPath2D(const std::vector<geometry_msgs::PoseStamped>&
// nav_msgs::Path poses2DToPath(const std::vector<geometry_msgs::Pose2D>& poses,
// const std::string& frame, const ros::Time& stamp)
// const std::string& frame, const robot::Time& stamp)
// {
// nav_msgs::Path path;
// path.poses.resize(poses.size());

0
src/Libraries/nav_2d_utils/src/nav_2d_utils/__init__.py
View File

131
src/Libraries/nav_2d_utils/src/nav_2d_utils/conversions.py
View File

@@ -1,131 +0,0 @@
from geometry_msgs.msg import Pose, Pose2D, Quaternion, Point
from nav_2d_msgs.msg import Twist2D, Path2D, Pose2DStamped, Point2D
from nav_msgs.msg import Path
from geometry_msgs.msg import Twist, PoseStamped
try:
from tf.transformations import euler_from_quaternion, quaternion_from_euler
def get_yaw(orientation):
rpy = euler_from_quaternion((orientation.x, orientation.y, orientation.z, orientation.w))
return rpy[2]
def from_yaw(yaw):
q = Quaternion()
q.x, q.y, q.z, q.w = quaternion_from_euler(0, 0, yaw)
return q
except ImportError:
from math import sin, cos, atan2
# From https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
def from_yaw(yaw):
q = Quaternion()
q.z = sin(yaw * 0.5)
q.w = cos(yaw * 0.5)
return q
def get_yaw(q):
siny_cosp = +2.0 * (q.w * q.z + q.x * q.y)
cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z)
return atan2(siny_cosp, cosy_cosp)
def twist2Dto3D(cmd_vel_2d):
cmd_vel = Twist()
cmd_vel.linear.x = cmd_vel_2d.x
cmd_vel.linear.y = cmd_vel_2d.y
cmd_vel.angular.z = cmd_vel_2d.theta
return cmd_vel
def twist3Dto2D(cmd_vel):
cmd_vel_2d = Twist2D()
cmd_vel_2d.x = cmd_vel.linear.x
cmd_vel_2d.y = cmd_vel.linear.y
cmd_vel_2d.theta = cmd_vel.angular.z
return cmd_vel_2d
def pointToPoint3D(point_2d):
point = Point()
point.x = point_2d.x
point.y = point_2d.y
return point
def pointToPoint2D(point):
point_2d = Point2D()
point_2d.x = point.x
point_2d.y = point.y
return point_2d
def poseToPose2D(pose):
pose2d = Pose2D()
pose2d.x = pose.position.x
pose2d.y = pose.position.y
pose2d.theta = get_yaw(pose.orientation)
return pose2d
def poseStampedToPose2DStamped(pose):
pose2d = Pose2DStamped()
pose2d.header = pose.header
pose2d.pose = poseToPose2D(pose.pose)
return pose2d
def poseToPose2DStamped(pose, frame, stamp):
pose2d = Pose2DStamped()
pose2d.header.frame_id = frame
pose2d.header.stamp = stamp
pose2d.pose = poseToPose2D(pose)
return pose2d
def pose2DToPose(pose2d):
pose = Pose()
pose.position.x = pose2d.x
pose.position.y = pose2d.y
pose.orientation = from_yaw(pose2d.theta)
return pose
def pose2DStampedToPoseStamped(pose2d):
pose = PoseStamped()
pose.header = pose2d.header
pose.pose = pose2DToPose(pose2d.pose)
return pose
def pose2DToPoseStamped(pose2d, frame, stamp):
pose = PoseStamped()
pose.header.frame_id = frame
pose.header.stamp = stamp
pose.pose.position.x = pose2d.x
pose.pose.position.y = pose2d.y
pose.pose.orientation = from_yaw(pose2d.theta)
return pose
def pathToPath2D(path):
path2d = Path2D()
if len(path.poses) == 0:
return path
path2d.header.frame_id = path.poses[0].header.frame_id
path2d.header.stamp = path.poses[0].header.stamp
for pose in path.poses:
stamped = poseStampedToPose2DStamped(pose)
path2d.poses.append(stamped.pose)
return path2d
def path2DToPath(path2d):
path = Path()
path.header = path2d.header
for pose2d in path2d.poses:
pose = PoseStamped()
pose.header = path2d.header
pose.pose = pose2DToPose(pose2d)
path.poses.append(pose)
return path

1
src/Libraries/nav_2d_utils/src/polygons.cpp
View File

@@ -37,6 +37,7 @@
#include <mapbox/earcut.hpp>
#include <algorithm>
#include <limits>
#include <sstream>
#include <string>
#include <vector>

2
src/Libraries/nav_2d_utils/test/param_tests.cpp
View File

@@ -140,7 +140,7 @@ TEST(Polygon2D, test_write)
int main(int argc, char** argv)
{
ros::init(argc, argv, "param_tests");
robot::init(argc, argv, "param_tests");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

2
src/Libraries/robot_cpp

Submodule src/Libraries/robot_cpp updated: 1974d0ddee...5b23baae3a

2
src/Libraries/tf3/CHANGELOG.rst
View File

@@ -309,7 +309,7 @@ Changelog for package tf2
------------------
* splitting rospy dependency into tf2_py so tf2 is pure c++ library.
* switching to console_bridge from rosconsole
* moving convert methods back into tf2 because it does not have any ros dependencies beyond ros::Time which is already a dependency of tf2
* moving convert methods back into tf2 because it does not have any ros dependencies beyond robot::Time which is already a dependency of tf2
* Cleaning up unnecessary dependency on roscpp
* Cleaning up packaging of tf2 including:
removing unused nodehandle

4
src/Libraries/tf3/include/tf3/buffer_core.h
View File

@@ -166,7 +166,7 @@ public:
geometry_msgs::Twist
lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
const tf::Point & reference_point, const std::string& reference_point_frame,
const ros::Time& time, const ros::Duration& averaging_interval) const;
const robot::Time& time, const robot::Duration& averaging_interval) const;
*/
/* \brief lookup the twist of the tracking frame with respect to the observational frame
*
@@ -183,7 +183,7 @@ public:
/*
geometry_msgs::Twist
lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
const ros::Time& time, const ros::Duration& averaging_interval) const;
const robot::Time& time, const robot::Duration& averaging_interval) const;
*/
/** \brief Test if a transform is possible
* \param target_frame The frame into which to transform

4
src/Libraries/tf3/include/tf3/compat.h
View File

@@ -1,4 +1,4 @@
// Compatibility types to avoid using ros:: or geometry_msgs:: namespaces inside tf3
// Compatibility types to avoid using robot:: or geometry_msgs:: namespaces inside tf3
#ifndef TF3_COMPAT_H
#define TF3_COMPAT_H
@@ -7,7 +7,7 @@
namespace tf3 {
// Minimal header/message equivalents owned by tf3 (no ros:: or geometry_msgs::)
// Minimal header/message equivalents owned by tf3 (no robot:: or geometry_msgs::)
struct HeaderMsg
{
uint32_t seq;

2
src/Libraries/tf3/include/tf3/exceptions.h
View File

@@ -37,7 +37,7 @@
namespace tf3{
/** \brief A base class for all tf3 exceptions
* This inherits from ros::exception
* This inherits from robot::exception
* which inherits from std::runtime_exception
*/
class TransformException: public std::runtime_error

8
src/Libraries/tf3/src/buffer_core.cpp
View File

@@ -674,8 +674,8 @@ tf3::TransformStampedMsg BufferCore::lookupTransform(const std::string& target_f
/*
geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
const std::string& observation_frame,
const ros::Time& time,
const ros::Duration& averaging_interval) const
const robot::Time& time,
const robot::Duration& averaging_interval) const
{
try
{
@@ -707,8 +707,8 @@ geometry_msgs::Twist BufferCore::lookupTwist(const std::string& tracking_frame,
const std::string& reference_frame,
const tf3::Point & reference_point,
const std::string& reference_point_frame,
const ros::Time& time,
const ros::Duration& averaging_interval) const
const robot::Time& time,
const robot::Duration& averaging_interval) const
{
try{
geometry_msgs::Twist t;

52
src/Libraries/tf3/test/cache_unittest.cpp
View File

@@ -82,7 +82,7 @@ TEST(TimeCache, Repeatability)
for ( uint64_t i = 1; i < runs ; i++ )
{
stor.frame_id_ = i;
stor.stamp_ = ros::Time().fromNSec(i);
stor.stamp_ = robot::Time().fromNSec(i);
cache.insertData(stor);
}
@@ -90,9 +90,9 @@ TEST(TimeCache, Repeatability)
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
cache.getData(robot::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
}
}
@@ -109,16 +109,16 @@ TEST(TimeCache, RepeatabilityReverseInsertOrder)
for ( int i = runs -1; i >= 0 ; i-- )
{
stor.frame_id_ = i;
stor.stamp_ = ros::Time().fromNSec(i);
stor.stamp_ = robot::Time().fromNSec(i);
cache.insertData(stor);
}
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
cache.getData(robot::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
}
}
@@ -143,16 +143,16 @@ TEST(TimeCache, RepeatabilityRandomInsertOrder)
ss << values[i];
stor.header.frame_id = ss.str();
stor.frame_id_ = i;
stor.stamp_ = ros::Time().fromNSec(i);
stor.stamp_ = robot::Time().fromNSec(i);
cache.insertData(stor);
}
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
cache.getData(robot::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
std::stringstream ss;
ss << values[i];
EXPECT_EQ(stor.header.frame_id, ss.str());
@@ -173,36 +173,36 @@ TEST(TimeCache, ZeroAtFront)
for ( uint64_t i = 1; i < runs ; i++ )
{
stor.frame_id_ = i;
stor.stamp_ = ros::Time().fromNSec(i);
stor.stamp_ = robot::Time().fromNSec(i);
cache.insertData(stor);
}
stor.frame_id_ = runs;
stor.stamp_ = ros::Time().fromNSec(runs);
stor.stamp_ = robot::Time().fromNSec(runs);
cache.insertData(stor);
for ( uint64_t i = 1; i < runs ; i++ )
{
cache.getData(ros::Time().fromNSec(i), stor);
cache.getData(robot::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
}
cache.getData(ros::Time(), stor);
cache.getData(robot::Time(), stor);
EXPECT_EQ(stor.frame_id_, runs);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs));
EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(runs));
stor.frame_id_ = runs;
stor.stamp_ = ros::Time().fromNSec(runs+1);
stor.stamp_ = robot::Time().fromNSec(runs+1);
cache.insertData(stor);
//Make sure we get a different value now that a new values is added at the front
cache.getData(ros::Time(), stor);
cache.getData(robot::Time(), stor);
EXPECT_EQ(stor.frame_id_, runs);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(runs+1));
EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(runs+1));
}
@@ -233,14 +233,14 @@ TEST(TimeCache, CartesianInterpolation)
stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
stor.frame_id_ = 2;
stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
stor.stamp_ = robot::Time().fromNSec(step * 100 + offset);
cache.insertData(stor);
}
for (int pos = 0; pos < 100 ; pos ++)
{
uint64_t time = offset + pos;
cache.getData(ros::Time().fromNSec(time), stor);
cache.getData(robot::Time().fromNSec(time), stor);
uint64_t time_out = stor.stamp_.toNSec();
double x_out = stor.translation_.x();
double y_out = stor.translation_.y();
@@ -286,13 +286,13 @@ TEST(TimeCache, ReparentingInterpolationProtection)
stor.translation_.setValue(xvalues[step], yvalues[step], zvalues[step]);
stor.frame_id_ = step + 4;
stor.stamp_ = ros::Time().fromNSec(step * 100 + offset);
stor.stamp_ = robot::Time().fromNSec(step * 100 + offset);
cache.insertData(stor);
}
for (int pos = 0; pos < 100 ; pos ++)
{
EXPECT_TRUE(cache.getData(ros::Time().fromNSec(offset + pos), stor));
EXPECT_TRUE(cache.getData(robot::Time().fromNSec(offset + pos), stor));
double x_out = stor.translation_.x();
double y_out = stor.translation_.y();
double z_out = stor.translation_.z();
@@ -354,14 +354,14 @@ TEST(TimeCache, AngularInterpolation)
quats[step].setRPY(yawvalues[step], pitchvalues[step], rollvalues[step]);
stor.rotation_ = quats[step];
stor.frame_id_ = 3;
stor.stamp_ = ros::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
stor.stamp_ = robot::Time().fromNSec(offset + (step * 100)); //step = 0 or 1
cache.insertData(stor);
}
for (int pos = 0; pos < 100 ; pos ++)
{
uint64_t time = offset + pos;
cache.getData(ros::Time().fromNSec(time), stor);
cache.getData(robot::Time().fromNSec(time), stor);
uint64_t time_out = stor.stamp_.toNSec();
tf3::Quaternion quat (stor.rotation_);
@@ -388,14 +388,14 @@ TEST(TimeCache, DuplicateEntries)
TransformStorage stor;
setIdentity(stor);
stor.frame_id_ = 3;
stor.stamp_ = ros::Time().fromNSec(1);
stor.stamp_ = robot::Time().fromNSec(1);
cache.insertData(stor);
cache.insertData(stor);
cache.getData(ros::Time().fromNSec(1), stor);
cache.getData(robot::Time().fromNSec(1), stor);
//printf(" stor is %f\n", stor.translation_.x());
EXPECT_TRUE(!std::isnan(stor.translation_.x()));

58
src/Libraries/tf3/test/simple_tf3_core.cpp
View File

@@ -46,7 +46,7 @@ TEST(tf3, setTransformValid)
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = ros::Time(1.0);
st.header.stamp = robot::Time(1.0);
st.child_frame_id = "child";
st.transform.rotation.w = 1;
EXPECT_TRUE(tfc.setTransform(st, "authority1"));
@@ -58,7 +58,7 @@ TEST(tf3, setTransformInvalidQuaternion)
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = ros::Time(1.0);
st.header.stamp = robot::Time(1.0);
st.child_frame_id = "child";
st.transform.rotation.w = 0;
EXPECT_FALSE(tfc.setTransform(st, "authority1"));
@@ -68,17 +68,17 @@ TEST(tf3, setTransformInvalidQuaternion)
TEST(tf3_lookupTransform, LookupException_Nothing_Exists)
{
tf3::BufferCore tfc;
EXPECT_THROW(tfc.lookupTransform("a", "b", ros::Time().fromSec(1.0)), tf3::LookupException);
EXPECT_THROW(tfc.lookupTransform("a", "b", robot::Time().fromSec(1.0)), tf3::LookupException);
}
TEST(tf3_canTransform, Nothing_Exists)
{
tf3::BufferCore tfc;
EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0)));
EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0)));
std::string error_msg = std::string();
EXPECT_FALSE(tfc.canTransform("a", "b", ros::Time().fromSec(1.0), &error_msg));
EXPECT_FALSE(tfc.canTransform("a", "b", robot::Time().fromSec(1.0), &error_msg));
ASSERT_STREQ(error_msg.c_str(), "canTransform: target_frame a does not exist. canTransform: source_frame b does not exist.");
}
@@ -88,11 +88,11 @@ TEST(tf3_lookupTransform, LookupException_One_Exists)
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = ros::Time(1.0);
st.header.stamp = robot::Time(1.0);
st.child_frame_id = "child";
st.transform.rotation.w = 1;
EXPECT_TRUE(tfc.setTransform(st, "authority1"));
EXPECT_THROW(tfc.lookupTransform("foo", "bar", ros::Time().fromSec(1.0)), tf3::LookupException);
EXPECT_THROW(tfc.lookupTransform("foo", "bar", robot::Time().fromSec(1.0)), tf3::LookupException);
}
@@ -101,11 +101,11 @@ TEST(tf3_canTransform, One_Exists)
tf3::BufferCore tfc;
geometry_msgs::TransformStamped st;
st.header.frame_id = "foo";
st.header.stamp = ros::Time(1.0);
st.header.stamp = robot::Time(1.0);
st.child_frame_id = "child";
st.transform.rotation.w = 1;
EXPECT_TRUE(tfc.setTransform(st, "authority1"));
EXPECT_FALSE(tfc.canTransform("foo", "bar", ros::Time().fromSec(1.0)));
EXPECT_FALSE(tfc.canTransform("foo", "bar", robot::Time().fromSec(1.0)));
}
TEST(tf3_chainAsVector, chain_v_configuration)
@@ -114,7 +114,7 @@ TEST(tf3_chainAsVector, chain_v_configuration)
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
st.header.stamp = ros::Time(0);
st.header.stamp = robot::Time(0);
st.transform.rotation.w = 1;
st.header.frame_id = "a";
@@ -136,34 +136,34 @@ TEST(tf3_chainAsVector, chain_v_configuration)
std::vector<std::string> chain;
mBC._chainAsVector( "c", ros::Time(), "c", ros::Time(), "c", chain);
mBC._chainAsVector( "c", robot::Time(), "c", robot::Time(), "c", chain);
EXPECT_EQ( 0, chain.size());
mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "c", chain);
mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "c", chain);
EXPECT_EQ( 3, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "c", chain);
mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "c", chain);
EXPECT_EQ( 3, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
mBC._chainAsVector( "a", ros::Time(), "c", ros::Time(), "a", chain);
mBC._chainAsVector( "a", robot::Time(), "c", robot::Time(), "a", chain);
EXPECT_EQ( 3, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "c" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "a" );
mBC._chainAsVector( "c", ros::Time(), "a", ros::Time(), "a", chain);
mBC._chainAsVector( "c", robot::Time(), "a", robot::Time(), "a", chain);
EXPECT_EQ( 3, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "a" );
if( chain.size() >= 2 ) EXPECT_EQ( chain[1], "b" );
if( chain.size() >= 3 ) EXPECT_EQ( chain[2], "c" );
mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "c", chain);
mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "c", chain);
EXPECT_EQ( 5, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
@@ -172,7 +172,7 @@ TEST(tf3_chainAsVector, chain_v_configuration)
if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "a", chain);
mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "a", chain);
EXPECT_EQ( 5, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
@@ -181,7 +181,7 @@ TEST(tf3_chainAsVector, chain_v_configuration)
if( chain.size() >= 4 ) EXPECT_EQ( chain[3], "b" );
if( chain.size() >= 5 ) EXPECT_EQ( chain[4], "c" );
mBC._chainAsVector( "c", ros::Time(), "e", ros::Time(), "e", chain);
mBC._chainAsVector( "c", robot::Time(), "e", robot::Time(), "e", chain);
EXPECT_EQ( 5, chain.size());
if( chain.size() >= 1 ) EXPECT_EQ( chain[0], "e" );
@@ -197,7 +197,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration)
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
st.header.stamp = ros::Time(0);
st.header.stamp = robot::Time(0);
st.transform.rotation.w = 1;
st.header.frame_id = "a";
@@ -217,7 +217,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration)
mBC.setTransform(st, "authority1");
std::vector<tf3::CompactFrameID> id_chain;
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
EXPECT_EQ(5, id_chain.size() );
if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
@@ -227,7 +227,7 @@ TEST(tf3_walkToTopParent, walk_i_configuration)
if( id_chain.size() >= 5 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[4]));
id_chain.clear();
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
EXPECT_EQ(5, id_chain.size() );
if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
@@ -244,7 +244,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
tf3::TestBufferCore tBC;
geometry_msgs::TransformStamped st;
st.header.stamp = ros::Time(0);
st.header.stamp = robot::Time(0);
st.transform.rotation.w = 1;
// st.header.frame_id = "aaa";
@@ -272,7 +272,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
mBC.setTransform(st, "authority1");
std::vector<tf3::CompactFrameID> id_chain;
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain);
tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("c"), 0, &id_chain);
EXPECT_EQ(5, id_chain.size());
if( id_chain.size() >= 1 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[0]));
@@ -281,7 +281,7 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
if( id_chain.size() >= 4 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[3]));
if( id_chain.size() >= 5 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[4]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain);
tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("c"), mBC._lookupFrameNumber("e"), 0, &id_chain);
EXPECT_EQ(5, id_chain.size());
if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
@@ -289,24 +289,24 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
if( id_chain.size() >= 4 ) EXPECT_EQ("b", tBC._lookupFrameString(mBC, id_chain[3]));
if( id_chain.size() >= 5 ) EXPECT_EQ("c", tBC._lookupFrameString(mBC, id_chain[4]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("a"), mBC._lookupFrameNumber("e"), 0, &id_chain);
EXPECT_EQ( id_chain.size(), 3 );
if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
if( id_chain.size() >= 3 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[2]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("a"), 0, &id_chain);
EXPECT_EQ( id_chain.size(), 3 );
if( id_chain.size() >= 1 ) EXPECT_EQ("a", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
if( id_chain.size() >= 3 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[2]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain);
tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("e"), mBC._lookupFrameNumber("d"), 0, &id_chain);
EXPECT_EQ( id_chain.size(), 2 );
if( id_chain.size() >= 1 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[1]));
tBC._walkToTopParent(mBC, ros::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain);
tBC._walkToTopParent(mBC, robot::Time(), mBC._lookupFrameNumber("d"), mBC._lookupFrameNumber("e"), 0, &id_chain);
EXPECT_EQ( id_chain.size(), 2 );
if( id_chain.size() >= 1 ) EXPECT_EQ("e", tBC._lookupFrameString(mBC, id_chain[0]));
if( id_chain.size() >= 2 ) EXPECT_EQ("d", tBC._lookupFrameString(mBC, id_chain[1]));
@@ -315,6 +315,6 @@ TEST(tf3_walkToTopParent, walk_v_configuration)
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
ros::Time::init(); //needed for ros::TIme::now()
robot::Time::init(); //needed for robot::TIme::now()
return RUN_ALL_TESTS();
}

92
src/Libraries/tf3/test/speed_test.cpp
View File

@@ -51,13 +51,13 @@ int main(int argc, char** argv)
tf3::BufferCore bc;
geometry_msgs::TransformStamped t;
t.header.stamp = ros::Time(1);
t.header.stamp = robot::Time(1);
t.header.frame_id = "root";
t.child_frame_id = "0";
t.transform.translation.x = 1;
t.transform.rotation.w = 1.0;
bc.setTransform(t, "me");
t.header.stamp = ros::Time(2);
t.header.stamp = robot::Time(2);
bc.setTransform(t, "me");
for (uint32_t i = 1; i < num_levels / 2; ++i)
@@ -69,7 +69,7 @@ int main(int argc, char** argv)
std::stringstream child_ss;
child_ss << i;
t.header.stamp = ros::Time(j);
t.header.stamp = robot::Time(j);
t.header.frame_id = parent_ss.str();
t.child_frame_id = child_ss.str();
bc.setTransform(t, "me");
@@ -79,10 +79,10 @@ int main(int argc, char** argv)
t.header.frame_id = "root";
std::stringstream ss;
ss << num_levels/2;
t.header.stamp = ros::Time(1);
t.header.stamp = robot::Time(1);
t.child_frame_id = ss.str();
bc.setTransform(t, "me");
t.header.stamp = ros::Time(2);
t.header.stamp = robot::Time(2);
bc.setTransform(t, "me");
for (uint32_t i = num_levels/2 + 1; i < num_levels; ++i)
@@ -94,7 +94,7 @@ int main(int argc, char** argv)
std::stringstream child_ss;
child_ss << i;
t.header.stamp = ros::Time(j);
t.header.stamp = robot::Time(j);
t.header.frame_id = parent_ss.str();
t.child_frame_id = child_ss.str();
bc.setTransform(t, "me");
@@ -113,13 +113,13 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(0));
out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(0));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("lookupTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
@@ -127,13 +127,13 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1));
out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("lookupTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
@@ -141,13 +141,13 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(1.5));
out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(1.5));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("lookupTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
@@ -155,13 +155,13 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
out_t = bc.lookupTransform(v_frame1, v_frame0, ros::Time(2));
out_t = bc.lookupTransform(v_frame1, v_frame0, robot::Time(2));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("lookupTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
@@ -169,13 +169,13 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(0));
bc.canTransform(v_frame1, v_frame0, robot::Time(0));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("canTransform at Time(0) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
@@ -183,13 +183,13 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(1));
bc.canTransform(v_frame1, v_frame0, robot::Time(1));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("canTransform at Time(1) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
@@ -197,13 +197,13 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(1.5));
bc.canTransform(v_frame1, v_frame0, robot::Time(1.5));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("canTransform at Time(1.5) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
@@ -211,13 +211,13 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(2));
bc.canTransform(v_frame1, v_frame0, robot::Time(2));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
//ROS_INFO_STREAM(out_t);
CONSOLE_BRIDGE_logInform("canTransform at Time(2) took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
@@ -225,27 +225,27 @@ int main(int argc, char** argv)
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(3));
bc.canTransform(v_frame1, v_frame0, robot::Time(3));
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
CONSOLE_BRIDGE_logInform("canTransform at Time(3) without error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif
#if 01
{
ros::WallTime start = ros::WallTime::now();
robot::WallTime start = robot::WallTime::now();
std::string str;
for (uint32_t i = 0; i < count; ++i)
{
bc.canTransform(v_frame1, v_frame0, ros::Time(3), &str);
bc.canTransform(v_frame1, v_frame0, robot::Time(3), &str);
}
ros::WallTime end = ros::WallTime::now();
ros::WallDuration dur = end - start;
robot::WallTime end = robot::WallTime::now();
robot::WallDuration dur = end - start;
CONSOLE_BRIDGE_logInform("canTransform at Time(3) with error string took %f for an average of %.9f", dur.toSec(), dur.toSec() / (double)count);
}
#endif

10
src/Libraries/tf3/test/static_cache_test.cpp
View File

@@ -56,14 +56,14 @@ TEST(StaticCache, Repeatability)
for ( uint64_t i = 1; i < runs ; i++ )
{
stor.frame_id_ = CompactFrameID(i);
stor.stamp_ = ros::Time().fromNSec(i);
stor.stamp_ = robot::Time().fromNSec(i);
cache.insertData(stor);
cache.getData(ros::Time().fromNSec(i), stor);
cache.getData(robot::Time().fromNSec(i), stor);
EXPECT_EQ(stor.frame_id_, i);
EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
EXPECT_EQ(stor.stamp_, robot::Time().fromNSec(i));
}
}
@@ -76,14 +76,14 @@ TEST(StaticCache, DuplicateEntries)
TransformStorage stor;
setIdentity(stor);
stor.frame_id_ = CompactFrameID(3);
stor.stamp_ = ros::Time().fromNSec(1);
stor.stamp_ = robot::Time().fromNSec(1);
cache.insertData(stor);
cache.insertData(stor);
cache.getData(ros::Time().fromNSec(1), stor);
cache.getData(robot::Time().fromNSec(1), stor);
//printf(" stor is %f\n", stor.transform.translation.x);
EXPECT_TRUE(!std::isnan(stor.translation_.x()));

14
src/Libraries/tf3/test/test_transform_datatypes.cpp
View File

@@ -35,8 +35,8 @@
TEST(Stamped, assignment)
{
tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> second("baz", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
tf3::Stamped<std::string> second("baz", robot::Time(0), "my_frame_id");
EXPECT_NE(second, first);
second = first;
@@ -45,8 +45,8 @@ TEST(Stamped, assignment)
TEST(Stamped, setData)
{
tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> second("baz", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
tf3::Stamped<std::string> second("baz", robot::Time(0), "my_frame_id");
EXPECT_NE(second, first);
second.setData("foobar");
@@ -55,7 +55,7 @@ TEST(Stamped, setData)
TEST(Stamped, copy_constructor)
{
tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
tf3::Stamped<std::string> second(first);
EXPECT_EQ(second, first);
@@ -63,7 +63,7 @@ TEST(Stamped, copy_constructor)
TEST(Stamped, default_constructor)
{
tf3::Stamped<std::string> first("foobar", ros::Time(0), "my_frame_id");
tf3::Stamped<std::string> first("foobar", robot::Time(0), "my_frame_id");
tf3::Stamped<std::string> second;
EXPECT_NE(second, first);
@@ -72,6 +72,6 @@ TEST(Stamped, default_constructor)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
ros::Time::init();
robot::Time::init();
return RUN_ALL_TESTS();
}