Replace uROS.cpp

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
 }