diff --git a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp index 35469e25..7be1633e 100644 --- a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp +++ b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp @@ -28,6 +28,8 @@ int main(int argc, char * argv[]) std::shared_ptr node = std::make_shared("ackermann_drive_test_node"); + node->set_parameter(rclcpp::Parameter("use_sim_time", true)); + auto publisher = node->create_publisher( "/ackermann_steering_controller/reference", 10); @@ -47,10 +49,12 @@ int main(int argc, char * argv[]) command.twist = tw; while (1) { - command.header.stamp = node->now(); - publisher->publish(command); - std::this_thread::sleep_for(50ms); rclcpp::spin_some(node); + if (node->get_clock()->started()) { + command.header.stamp = node->now(); + publisher->publish(command); + } + std::this_thread::sleep_for(50ms); } rclcpp::shutdown(); diff --git a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf index 99819e25..bcdf8205 100644 --- a/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf +++ b/gz_ros2_control_demos/urdf/test_ackermann_drive.xacro.urdf @@ -34,8 +34,8 @@ - - + + @@ -60,8 +60,8 @@ - - + + @@ -87,8 +87,8 @@ - - + + @@ -108,12 +108,13 @@ - - + + + @@ -124,12 +125,13 @@ - - + + + @@ -146,8 +148,8 @@ - - + + @@ -173,8 +175,8 @@ - - + +