Skip to content

Commit

Permalink
Addressing IntelRealSense#2645 using the solution suggested by @ascte…
Browse files Browse the repository at this point in the history
…c-andre
  • Loading branch information
dorodnic committed Nov 28, 2018
1 parent bb458b7 commit db66f54
Show file tree
Hide file tree
Showing 157 changed files with 1,066 additions and 1,066 deletions.
10 changes: 5 additions & 5 deletions src/media/ros/ros_file_format.h
Original file line number Diff line number Diff line change
Expand Up @@ -533,21 +533,21 @@ namespace librealsense
return device_serializer::nanoseconds::min();
}

inline device_serializer::nanoseconds to_nanoseconds(const ros::Time& t)
inline device_serializer::nanoseconds to_nanoseconds(const rs2rosinternal::Time& t)
{
if (t == ros::TIME_MIN)
if (t == rs2rosinternal::TIME_MIN)
return get_static_file_info_timestamp();

return device_serializer::nanoseconds(t.toNSec());
}

inline ros::Time to_rostime(const device_serializer::nanoseconds& t)
inline rs2rosinternal::Time to_rostime(const device_serializer::nanoseconds& t)
{
if (t == get_static_file_info_timestamp())
return ros::TIME_MIN;
return rs2rosinternal::TIME_MIN;

auto secs = std::chrono::duration_cast<std::chrono::duration<double>>(t);
return ros::Time(secs.count());
return rs2rosinternal::Time(secs.count());
}

namespace legacy_file_format
Expand Down
14 changes: 7 additions & 7 deletions src/media/ros/ros_reader.h
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,7 @@ namespace librealsense
throw invalid_value_exception(to_string() << "Requested time is out of playback length. (Requested = " << seek_time.count() << ", Duration = " << m_total_duration.count() << ")");
}
auto seek_time_as_secs = std::chrono::duration_cast<std::chrono::duration<double>>(seek_time);
auto seek_time_as_rostime = ros::Time(seek_time_as_secs.count());
auto seek_time_as_rostime = rs2rosinternal::Time(seek_time_as_secs.count());

m_samples_view.reset(new rosbag::View(m_file, FalseQuery()));

Expand All @@ -119,7 +119,7 @@ namespace librealsense
{
view.addQuery(m_file, rosbag::TopicQuery(topic), start_time, as_rostime);
}
std::map<device_serializer::stream_identifier, ros::Time> last_frames;
std::map<device_serializer::stream_identifier, rs2rosinternal::Time> last_frames;
for (auto&& m : view)
{
if (m.isType<sensor_msgs::Image>() || m.isType<sensor_msgs::Imu>())
Expand Down Expand Up @@ -156,7 +156,7 @@ namespace librealsense

virtual void enable_stream(const std::vector<device_serializer::stream_identifier>& stream_ids) override
{
ros::Time start_time = ros::TIME_MIN + ros::Duration{ 0, 1 }; //first non 0 timestamp and afterward
rs2rosinternal::Time start_time = rs2rosinternal::TIME_MIN + rs2rosinternal::Duration{ 0, 1 }; //first non 0 timestamp and afterward
if (m_samples_view == nullptr) //Starting to stream
{
m_samples_view = std::unique_ptr<rosbag::View>(new rosbag::View(m_file, FalseQuery()));
Expand Down Expand Up @@ -204,7 +204,7 @@ namespace librealsense
{
return;
}
ros::Time curr_time;
rs2rosinternal::Time curr_time;
if (m_samples_itrator == m_samples_view->end())
{
curr_time = m_samples_view->getEndTime();
Expand Down Expand Up @@ -248,7 +248,7 @@ namespace librealsense
{
throw io_exception(to_string()
<< "Invalid file format, expected "
<< ros::message_traits::DataType<ROS_TYPE>::value()
<< rs2rosinternal::message_traits::DataType<ROS_TYPE>::value()
<< " message but got: " << msg.getDataType()
<< "(Topic: " << msg.getTopic() << ")");
}
Expand Down Expand Up @@ -1038,8 +1038,8 @@ namespace librealsense
{
throw io_exception(to_string()
<< "Invalid file format, expected "
<< ros::message_traits::DataType<realsense_legacy_msgs::motion_stream_info>::value()
<< " or " << ros::message_traits::DataType<realsense_legacy_msgs::stream_info>::value()
<< rs2rosinternal::message_traits::DataType<realsense_legacy_msgs::motion_stream_info>::value()
<< " or " << rs2rosinternal::message_traits::DataType<realsense_legacy_msgs::stream_info>::value()
<< " message but got: " << infos_msg.getDataType()
<< "(Topic: " << infos_msg.getTopic() << ")");
}
Expand Down
6 changes: 3 additions & 3 deletions src/media/ros/ros_writer.h
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,7 @@ namespace librealsense
noti_msg.severity = get_string(n.severity);
noti_msg.description = n.description;
auto secs = std::chrono::duration_cast<std::chrono::duration<double>>(std::chrono::duration<double, std::nano>(n.timestamp));
noti_msg.timestamp = ros::Time(secs.count());
noti_msg.timestamp = rs2rosinternal::Time(secs.count());
noti_msg.serialized_data = n.serialized_data;
return noti_msg;
}
Expand Down Expand Up @@ -192,7 +192,7 @@ namespace librealsense
image.data.assign(p_data, p_data + size);
image.header.seq = static_cast<uint32_t>(vid_frame->get_frame_number());
std::chrono::duration<double, std::milli> timestamp_ms(vid_frame->get_frame_timestamp());
image.header.stamp = ros::Time(std::chrono::duration<double>(timestamp_ms).count());
image.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count());
std::string TODO_CORRECT_ME = "0";
image.header.frame_id = TODO_CORRECT_ME;
auto image_topic = ros_topic::frame_data_topic(stream_id);
Expand All @@ -210,7 +210,7 @@ namespace librealsense

imu_msg.header.seq = static_cast<uint32_t>(frame.frame->get_frame_number());
std::chrono::duration<double, std::milli> timestamp_ms(frame.frame->get_frame_timestamp());
imu_msg.header.stamp = ros::Time(std::chrono::duration<double>(timestamp_ms).count());
imu_msg.header.stamp = rs2rosinternal::Time(std::chrono::duration<double>(timestamp_ms).count());
std::string TODO_CORRECT_ME = "0";
imu_msg.header.frame_id = TODO_CORRECT_ME;
auto data_ptr = reinterpret_cast<const float*>(frame.frame->get_frame_data());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,7 @@
//#include <boost/shared_ptr.hpp>


namespace ros {
namespace rs2rosinternal {

typedef std::vector<std::pair<std::string, std::string> > VP_string;
typedef std::vector<std::string> V_string;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
#include <vector>
#include "cpp_common_decl.h"

namespace ros
namespace rs2rosinternal
{

namespace debug
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@

#include <stdexcept>

namespace ros
namespace rs2rosinternal
{

/**
Expand All @@ -44,7 +44,7 @@ class Exception : public std::runtime_error
{}
};

} // namespace ros
} // namespace rs2rosinternal

#endif

Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@
#include "../../../cpp_common/include/ros/datatypes.h"
#include "cpp_common_decl.h"

namespace ros
namespace rs2rosinternal
{

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@
#endif
#include <string>

namespace ros {
namespace rs2rosinternal {

/**
* Convenient cross platform function for returning a std::string of an
Expand All @@ -79,6 +79,6 @@ inline bool get_environment_variable(std::string &str, const char* environment_v
}
}

} // namespace ros
} // namespace rs2rosinternal

#endif /* CPP_COMMON_PLATFORM_H_ */
2 changes: 1 addition & 1 deletion third-party/realsense-file/rosbag/cpp_common/src/debug.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@
#include <cstdio>
#include <sstream>

namespace ros
namespace rs2rosinternal
{
namespace debug
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@

using namespace std;

namespace ros
namespace rs2rosinternal
{

Header::Header()
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,13 +61,13 @@ typedef std::shared_ptr< ::diagnostic_msgs::KeyValue const> KeyValueConstPtr;
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::diagnostic_msgs::KeyValue_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::diagnostic_msgs::KeyValue_<ContainerAllocator> >::stream(s, "", v);
rs2rosinternal::message_operations::Printer< ::diagnostic_msgs::KeyValue_<ContainerAllocator> >::stream(s, "", v);
return s;
}

} // namespace diagnostic_msgs

namespace ros
namespace rs2rosinternal
{
namespace message_traits
{
Expand Down Expand Up @@ -151,9 +151,9 @@ string value # a value to track over time\n\
};

} // namespace message_traits
} // namespace ros
} // namespace rs2rosinternal

namespace ros
namespace rs2rosinternal
{
namespace serialization
{
Expand All @@ -170,9 +170,9 @@ namespace serialization
}; // struct KeyValue_

} // namespace serialization
} // namespace ros
} // namespace rs2rosinternal

namespace ros
namespace rs2rosinternal
{
namespace message_operations
{
Expand All @@ -190,6 +190,6 @@ struct Printer< ::diagnostic_msgs::KeyValue_<ContainerAllocator> >
};

} // namespace message_operations
} // namespace ros
} // namespace rs2rosinternal

#endif // DIAGNOSTIC_MSGS_MESSAGE_KEYVALUE_H
14 changes: 7 additions & 7 deletions third-party/realsense-file/rosbag/msgs/geometry_msgs/Accel.h
Original file line number Diff line number Diff line change
Expand Up @@ -63,13 +63,13 @@ typedef std::shared_ptr< ::geometry_msgs::Accel const> AccelConstPtr;
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Accel_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::geometry_msgs::Accel_<ContainerAllocator> >::stream(s, "", v);
rs2rosinternal::message_operations::Printer< ::geometry_msgs::Accel_<ContainerAllocator> >::stream(s, "", v);
return s;
}

} // namespace geometry_msgs

namespace ros
namespace rs2rosinternal
{
namespace message_traits
{
Expand Down Expand Up @@ -167,9 +167,9 @@ float64 z\n\
};

} // namespace message_traits
} // namespace ros
} // namespace rs2rosinternal

namespace ros
namespace rs2rosinternal
{
namespace serialization
{
Expand All @@ -186,9 +186,9 @@ namespace serialization
}; // struct Accel_

} // namespace serialization
} // namespace ros
} // namespace rs2rosinternal

namespace ros
namespace rs2rosinternal
{
namespace message_operations
{
Expand All @@ -208,6 +208,6 @@ struct Printer< ::geometry_msgs::Accel_<ContainerAllocator> >
};

} // namespace message_operations
} // namespace ros
} // namespace rs2rosinternal

#endif // GEOMETRY_MSGS_MESSAGE_ACCEL_H
Original file line number Diff line number Diff line change
Expand Up @@ -63,13 +63,13 @@ typedef std::shared_ptr< ::geometry_msgs::AccelStamped const> AccelStampedConstP
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::AccelStamped_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::geometry_msgs::AccelStamped_<ContainerAllocator> >::stream(s, "", v);
rs2rosinternal::message_operations::Printer< ::geometry_msgs::AccelStamped_<ContainerAllocator> >::stream(s, "", v);
return s;
}

} // namespace geometry_msgs

namespace ros
namespace rs2rosinternal
{
namespace message_traits
{
Expand Down Expand Up @@ -191,9 +191,9 @@ float64 z\n\
};

} // namespace message_traits
} // namespace ros
} // namespace rs2rosinternal

namespace ros
namespace rs2rosinternal
{
namespace serialization
{
Expand All @@ -210,9 +210,9 @@ namespace serialization
}; // struct AccelStamped_

} // namespace serialization
} // namespace ros
} // namespace rs2rosinternal

namespace ros
namespace rs2rosinternal
{
namespace message_operations
{
Expand All @@ -232,6 +232,6 @@ struct Printer< ::geometry_msgs::AccelStamped_<ContainerAllocator> >
};

} // namespace message_operations
} // namespace ros
} // namespace rs2rosinternal

#endif // GEOMETRY_MSGS_MESSAGE_ACCELSTAMPED_H
Original file line number Diff line number Diff line change
Expand Up @@ -64,13 +64,13 @@ typedef std::shared_ptr< ::geometry_msgs::AccelWithCovariance const> AccelWithCo
template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::AccelWithCovariance_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::geometry_msgs::AccelWithCovariance_<ContainerAllocator> >::stream(s, "", v);
rs2rosinternal::message_operations::Printer< ::geometry_msgs::AccelWithCovariance_<ContainerAllocator> >::stream(s, "", v);
return s;
}

} // namespace geometry_msgs

namespace ros
namespace rs2rosinternal
{
namespace message_traits
{
Expand Down Expand Up @@ -180,9 +180,9 @@ float64 z\n\
};

} // namespace message_traits
} // namespace ros
} // namespace rs2rosinternal

namespace ros
namespace rs2rosinternal
{
namespace serialization
{
Expand All @@ -199,9 +199,9 @@ namespace serialization
}; // struct AccelWithCovariance_

} // namespace serialization
} // namespace ros
} // namespace rs2rosinternal

namespace ros
namespace rs2rosinternal
{
namespace message_operations
{
Expand All @@ -224,6 +224,6 @@ struct Printer< ::geometry_msgs::AccelWithCovariance_<ContainerAllocator> >
};

} // namespace message_operations
} // namespace ros
} // namespace rs2rosinternal

#endif // GEOMETRY_MSGS_MESSAGE_ACCELWITHCOVARIANCE_H
Loading

0 comments on commit db66f54

Please sign in to comment.