ROS2Clock.cpp 1.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566
  1. /*
  2. * Copyright (c) Contributors to the Open 3D Engine Project.
  3. * For complete copyright and license terms please see the LICENSE at the root of this distribution.
  4. *
  5. * SPDX-License-Identifier: Apache-2.0 OR MIT
  6. *
  7. */
  8. #include <ROS2/Clock/ROS2Clock.h>
  9. #include <ROS2/Clock/SimulationTimeSource.h>
  10. #include <ROS2/ROS2Bus.h>
  11. #include <rclcpp/qos.hpp>
  12. namespace ROS2
  13. {
  14. ROS2Clock::ROS2Clock()
  15. : m_timeSource(AZStd::make_unique<SimulationTimeSource>())
  16. , m_publishClock(true)
  17. {
  18. }
  19. ROS2Clock::ROS2Clock(AZStd::unique_ptr<ITimeSource> timeSource, bool publishClock)
  20. : m_timeSource(AZStd::move(timeSource))
  21. , m_publishClock(publishClock)
  22. {
  23. }
  24. void ROS2Clock::Activate()
  25. {
  26. m_timeSource->Activate();
  27. }
  28. void ROS2Clock::Deactivate()
  29. {
  30. m_timeSource->Deactivate();
  31. }
  32. builtin_interfaces::msg::Time ROS2Clock::GetROSTimestamp() const
  33. {
  34. return m_timeSource->GetROSTimestamp();
  35. }
  36. void ROS2Clock::Tick()
  37. {
  38. if (!m_publishClock)
  39. {
  40. return;
  41. }
  42. if (!m_clockPublisher)
  43. { // Lazy construct
  44. auto ros2Node = ROS2Interface::Get()->GetNode();
  45. rclcpp::ClockQoS qos;
  46. m_clockPublisher = ros2Node->create_publisher<rosgraph_msgs::msg::Clock>("/clock", qos);
  47. }
  48. rosgraph_msgs::msg::Clock msg;
  49. msg.clock = GetROSTimestamp();
  50. m_clockPublisher->publish(msg);
  51. }
  52. AZ::Outcome<void, AZStd::string> ROS2Clock::AdjustTime(const builtin_interfaces::msg::Time & time) const
  53. {
  54. return m_timeSource->AdjustTime(time);
  55. }
  56. } // namespace ROS2