From 43e7ae22d6a860e9e0a6038af55e07db5334b6a8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Malthe=20Faurschou=20T=C3=B8ttrup?= <201907882@post.au.dk>
Date: Thu, 18 Aug 2022 14:59:27 +0200
Subject: [PATCH] Replace uROS.cpp
---
.../Core/Src/DRobotti/uROS.cpp | 50 +++++++------------
1 file changed, 17 insertions(+), 33 deletions(-)
diff --git a/DRobotti_SpeedControl/Core/Src/DRobotti/uROS.cpp b/DRobotti_SpeedControl/Core/Src/DRobotti/uROS.cpp
index ca5c987..4c77805 100644
--- a/DRobotti_SpeedControl/Core/Src/DRobotti/uROS.cpp
+++ b/DRobotti_SpeedControl/Core/Src/DRobotti/uROS.cpp
@@ -34,8 +34,8 @@ std_msgs__msg__Int64 encoder_tick_stamp_message;
std_msgs__msg__Int64 motor_stopped_timestamp;
// Variables
-int64_t move_message_time;
-int64_t stop_time;
+int64_t move_message_time = 0;
+int64_t stop_time = 0;
rcl_allocator_t configure_uROS(void){
@@ -75,38 +75,32 @@ void uROS_SetupAndRunNode(void)
}
// Create node
- rclc_node_init_default(&node, "rear_wheel_speed_control_robot1", "", &support);
+ rclc_node_init_default(&node, "rear_wheel_speed_control_robot2", "", &support);
// Create publisher
rclc_publisher_init_best_effort(
&encoder_tick_stamp_publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64),
- "robotti1/rear_wheel_encoder/stamp_nanosec");
+ "robotti2/rear_wheel_encoder/stamp_nanosec");
rclc_publisher_init_best_effort(
&encoder_tick_right_publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
- "robotti1/rear_wheel_encoder/right");
+ "robotti2/rear_wheel_encoder/right");
rclc_publisher_init_best_effort(
&encoder_tick_left_publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
- "robotti1/rear_wheel_encoder/left");
+ "robotti2/rear_wheel_encoder/left");
// Create a timer for the publisher
rclc_timer_init_default(&rear_wheel_state_timer,
&support,
- RCL_MS_TO_NS(10),
+ RCL_MS_TO_NS(10), //this timer is started every 0.01 seconds
&EncoderDataPublish_cb);
- // Create timer for move messages
- rclc_timer_init_default(&move_message_timer,
- &support,
- RCL_MS_TO_NS(1000), //DONT KNOW IF THIS WORKS, we want one second
- &MovTimer_cb);
-
// Synchronize epoch timestamp from pi.
rmw_uros_sync_session(1000);
@@ -115,21 +109,21 @@ void uROS_SetupAndRunNode(void)
&motors_stopped_publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int64),
- "robotti1/rw_motors_stopped_timestamp_ns");
+ "robotti2/rw_motors_stopped_timestamp_ns");
// Create move cmd subscriber
rclc_subscription_init_best_effort(
&move_cmd_subscriber,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(ackermann_msgs, msg, AckermannDrive),
- "robotti1/move_cmd");
+ "robotti2/move_cmd");
// Create em stop subscriber
rclc_subscription_init_best_effort(
&em_stop_subscriber,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool),
- "robotti1/em_stop");
+ "robotti2/em_stop");
rclc_executor_t executor;
executor = rclc_executor_get_zero_initialized_executor();
@@ -142,10 +136,6 @@ void uROS_SetupAndRunNode(void)
rclc_executor_add_subscription(&executor, &em_stop_subscriber, &em_stop_message, &onEmStop_cb, ON_NEW_DATA);
rclc_executor_add_timer(&executor, &rear_wheel_state_timer);
- // is this how you get time?
- stop_time = rmw_uros_epoch_nanos();
- rclc_executor_add_timer(&executor, &move_message_timer);
-
for(;;)
{
rclc_executor_spin(&executor); // Run the executer
@@ -194,19 +184,13 @@ void EncoderDataPublish_cb(rcl_timer_t* timer, int64_t last_call_time){
rcl_publish(&encoder_tick_left_publisher, &encoder_tick_left_message, NULL);
rcl_publish(&encoder_tick_right_publisher, &encoder_tick_right_message, NULL);
rcl_publish(&encoder_tick_stamp_publisher, &encoder_tick_stamp_message, NULL);
- }
- }
-}
-/*
- Times when there was last sent a move message.
- If no messages are being sent to the move topic the robot should stop,
- as this might indicate that the connection with the host computer is lost.
- */
-void MovTimer_cb(rcl_timer_t* timer){
- if(stop_time - move_message_time > 1000000000){ //it is more than one second since a message was last received, note time is in NS
- SharedResources::GetInstance().SetTarget(0,0);
+ //if it is more than one 1 second since a message was last received,
+ //we want to stop as this might indicated that the connection with host computer is lost.
+ if(stop_time - move_message_time > 1000000000){ //note time is in NS
+ SharedResources::GetInstance().SetTarget(0.0,0.0);
+ }
+ stop_time = encoder_tick_stamp_message.data;
+ }
}
- stop_time = rmw_uros_epoch_nanos();
- //TODO: create timer again - dont know if this is already done by the executor in the inf loop
}
--
GitLab