rebase to show diff with #125

Signed-off-by: Michael Anderson <[email protected]>

.github/workflows/build-and-test.sh

@@ -51,5 +51,6 @@ colcon build --packages-up-to buoy_tests --event-handlers console_direct+
 source $COLCON_WS/install/setup.bash
 
 # Test all buoy packages
-colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+
+#colcon test --packages-select-regex=buoy --packages-skip=buoy_msgs --event-handlers console_direct+
+launch_test install/buoy_tests/share/buoy_tests/launch/pc_commands_ros_feedback.launch.py
 colcon test-result

buoy_gazebo/src/controllers/PowerController/PowerController.cpp

@@ -262,10 +262,20 @@ struct PowerControllerPrivate
     int8_t result = buoy_interfaces::msg::PBCommandResponse::OK;
 
     // high priority cmd access
+    RCLCPP_INFO_STREAM(
+      ros_->node_->get_logger(),
+      "[ROS 2 Power Control] Command waiting for 'next' lock");
     std::unique_lock next_lock(services_->next_access_mutex_);
+    RCLCPP_INFO_STREAM(
+      ros_->node_->get_logger(),
+      "[ROS 2 Power Control] Command waiting for 'command' lock");
     std::unique_lock cmd_lock(services_->command_mutex_);
     next_lock.unlock();
 
+    RCLCPP_INFO_STREAM(
+      ros_->node_->get_logger(),
+      "[ROS 2 Power Control] Command processing");
+
     if (valid_range.from_value > command_value ||
       command_value > valid_range.to_value)
     {
@@ -296,7 +306,7 @@ struct PowerControllerPrivate
       [this](const std::shared_ptr<buoy_interfaces::srv::PCWindCurrCommand::Request> request,
         std::shared_ptr<buoy_interfaces::srv::PCWindCurrCommand::Response> response)
       {
-        RCLCPP_DEBUG_STREAM(
+        RCLCPP_INFO_STREAM(
           ros_->node_->get_logger(),
           "[ROS 2 Power Control] PCWindCurrCommand Received [" << request->wind_curr << " Amps]");
 
@@ -314,6 +324,9 @@ struct PowerControllerPrivate
               services_->valid_wind_curr_range_.from_value << ", " <<
               services_->valid_wind_curr_range_.to_value << "] Amps");
         }
+        RCLCPP_INFO_STREAM(
+          ros_->node_->get_logger(),
+          "[ROS 2 Power Control] PCWindCurrCommand Done [" << (int)response->result.value << "]");
       };
     services_->torque_command_service_ =
       ros_->node_->create_service<buoy_interfaces::srv::PCWindCurrCommand>(
@@ -344,6 +357,9 @@ struct PowerControllerPrivate
               services_->valid_scale_range_.from_value << ", " <<
               services_->valid_scale_range_.to_value << "]");
         }
+        RCLCPP_INFO_STREAM(
+          ros_->node_->get_logger(),
+          "[ROS 2 Power Control] PCScaleCommand Done [" << (int)response->result.value << "]");
       };
     services_->scale_command_service_ =
       ros_->node_->create_service<buoy_interfaces::srv::PCScaleCommand>(
@@ -374,6 +390,9 @@ struct PowerControllerPrivate
               services_->valid_retract_range_.from_value << ", " <<
               services_->valid_retract_range_.to_value << "]");
         }
+        RCLCPP_INFO_STREAM(
+          ros_->node_->get_logger(),
+          "[ROS 2 Power Control] PCRetractCommand Done [" << (int)response->result.value << "]");
       };
     services_->retract_command_service_ =
       ros_->node_->create_service<buoy_interfaces::srv::PCRetractCommand>(
@@ -386,7 +405,7 @@ struct PowerControllerPrivate
       [this](const std::shared_ptr<buoy_interfaces::srv::PCBiasCurrCommand::Request> request,
         std::shared_ptr<buoy_interfaces::srv::PCBiasCurrCommand::Response> response)
       {
-        RCLCPP_DEBUG_STREAM(
+        RCLCPP_INFO_STREAM(
           ros_->node_->get_logger(),
           "[ROS 2 Power Control] PCBiasCurrCommand Received [" << request->bias_curr << " Amps]");
 
@@ -404,6 +423,9 @@ struct PowerControllerPrivate
               services_->valid_bias_curr_range_.from_value << ", " <<
               services_->valid_bias_curr_range_.to_value << "]");
         }
+        RCLCPP_INFO_STREAM(
+          ros_->node_->get_logger(),
+          "[ROS 2 Power Control] PCBiasCurrCommand Done [" << (int)response->result.value << "]");
       };
     services_->bias_curr_command_service_ =
       ros_->node_->create_service<buoy_interfaces::srv::PCBiasCurrCommand>(

buoy_tests/CMakeLists.txt

@@ -89,7 +89,7 @@ if(BUILD_TESTING)
 
     if(buoy_add_gtest_LAUNCH_TEST)
       add_launch_test(launch/${TEST_NAME}.launch.py
-        TIMEOUT 650
+        TIMEOUT 1000
       )
     else()
       ament_add_gtest_test(${TEST_NAME})
@@ -128,7 +128,7 @@ if(BUILD_TESTING)
     endif()
 
     add_launch_test(launch/${PYTEST_NAME}_py.launch.py
-      TIMEOUT 650
+      TIMEOUT 1000
     )
   endfunction()
 

buoy_tests/launch/pc_commands_ros_feedback.launch.py

@@ -59,7 +59,7 @@ def generate_test_description():
 class PCCommandsROSTest(unittest.TestCase):
 
     def test_termination(self, gazebo_test_fixture, proc_info):
-        proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=600)
+        proc_info.assertWaitForShutdown(process=gazebo_test_fixture, timeout=975)
 
 
 @launch_testing.post_shutdown_test()

buoy_tests/tests/pc_commands_ros_feedback.cpp

@@ -74,10 +74,10 @@ class PCROSNode final : public buoy_api::Interface<PCROSNode>
 
   PBTorqueControlPolicy torque_policy_;
 
-  PCWindCurrServiceResponseFuture pc_wind_curr_response_future_;
-  PCBiasCurrServiceResponseFuture pc_bias_curr_response_future_;
-  PCScaleServiceResponseFuture pc_scale_response_future_;
-  PCRetractServiceResponseFuture pc_retract_response_future_;
+  // PCWindCurrServiceResponseFuture pc_wind_curr_response_future_;
+  // PCBiasCurrServiceResponseFuture pc_bias_curr_response_future_;
+  // PCScaleServiceResponseFuture pc_scale_response_future_;
+  // PCRetractServiceResponseFuture pc_retract_response_future_;
 
   explicit PCROSNode(const std::string & node_name)
   : buoy_api::Interface<PCROSNode>(node_name)
@@ -308,15 +308,20 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback)
   EXPECT_NE(node->wind_curr_, wc);
 
   // Now send wind curr command
-  node->pc_wind_curr_response_future_ = node->send_pc_wind_curr_command(wc);
-  EXPECT_TRUE(node->pc_wind_curr_response_future_.valid()) << "Winding Current future invalid!";
-  node->pc_wind_curr_response_future_.wait();
+  PCROSNode::PCWindCurrServiceResponseFuture pc_wind_curr_response_future =
+    node->send_pc_wind_curr_command(wc);
+  EXPECT_TRUE(pc_wind_curr_response_future.valid()) << "Winding Current future invalid!";
+  std::cout << "wind curr: before wait future" << std::endl;
+  pc_wind_curr_response_future.wait();
+  std::cout << "wind curr: after wait future" << std::endl;
   EXPECT_EQ(
-    node->pc_wind_curr_response_future_.get()->result.value,
-    node->pc_wind_curr_response_future_.get()->result.OK);
+    pc_wind_curr_response_future.get()->result.value,
+    pc_wind_curr_response_future.get()->result.OK);
 
   // Run a bit for wind curr command to process
+  std::cout << "wind curr: before server run" << std::endl;
   fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/);
+  std::cout << "wind curr: after server run" << std::endl;
   EXPECT_EQ(preCmdIterations + 2 * feedbackCheckIterations, iterations);
 
   std::this_thread::sleep_for(500ms);
@@ -333,15 +338,20 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback)
   EXPECT_NE(node->scale_, scale);
 
   // Now send scale command
-  node->pc_scale_response_future_ = node->send_pc_scale_command(scale);
-  EXPECT_TRUE(node->pc_scale_response_future_.valid()) << "Scale future invalid!";
-  node->pc_scale_response_future_.wait();
+  PCROSNode::PCScaleServiceResponseFuture pc_scale_response_future =
+    node->send_pc_scale_command(scale);
+  EXPECT_TRUE(pc_scale_response_future.valid()) << "Scale future invalid!";
+  std::cout << "scale: before wait future" << std::endl;
+  pc_scale_response_future.wait();
+  std::cout << "scale: after wait future" << std::endl;
   EXPECT_EQ(
-    node->pc_scale_response_future_.get()->result.value,
-    node->pc_scale_response_future_.get()->result.OK);
+    pc_scale_response_future.get()->result.value,
+    pc_scale_response_future.get()->result.OK);
 
   // Run a bit for scale command to process
+  std::cout << "scale: before server run" << std::endl;
   fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/);
+  std::cout << "scale: after server run" << std::endl;
   EXPECT_EQ(preCmdIterations + 3 * feedbackCheckIterations, iterations);
 
   std::this_thread::sleep_for(500ms);
@@ -358,15 +368,20 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback)
   EXPECT_NE(node->retract_, retract);
 
   // Now send retract command
-  node->pc_retract_response_future_ = node->send_pc_retract_command(retract);
-  EXPECT_TRUE(node->pc_retract_response_future_.valid()) << "Retract future invalid!";
-  node->pc_retract_response_future_.wait();
+  PCROSNode::PCRetractServiceResponseFuture pc_retract_response_future =
+    node->send_pc_retract_command(retract);
+  EXPECT_TRUE(pc_retract_response_future.valid()) << "Retract future invalid!";
+  std::cout << "retract: before wait future" << std::endl;
+  pc_retract_response_future.wait();
+  std::cout << "retract: after wait future" << std::endl;
   EXPECT_EQ(
-    node->pc_retract_response_future_.get()->result.value,
-    node->pc_retract_response_future_.get()->result.OK);
+    pc_retract_response_future.get()->result.value,
+    pc_retract_response_future.get()->result.OK);
 
   // Run a bit for retract command to process
+  std::cout << "retract: before server run" << std::endl;
   fixture->Server()->Run(true /*blocking*/, feedbackCheckIterations, false /*paused*/);
+  std::cout << "retract: after server run" << std::endl;
   EXPECT_EQ(preCmdIterations + 4 * feedbackCheckIterations, iterations);
 
   std::this_thread::sleep_for(500ms);
@@ -380,9 +395,11 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback)
   ///////////////////////////////////////////////////////
   // Check Return to Default Winding Current Damping
   int torque_timeout_iterations{2000};
+  std::cout << "wind curr timeout: before server run" << std::endl;
   fixture->Server()->Run(
     true /*blocking*/,
     torque_timeout_iterations - 2 * feedbackCheckIterations, false /*paused*/);
+  std::cout << "wind curr timeout: after server run" << std::endl;
   EXPECT_EQ(
     preCmdIterations + 2 * feedbackCheckIterations + torque_timeout_iterations,
     iterations);
@@ -407,16 +424,21 @@ TEST_F(BuoyPCTests, PCCommandsInROSFeedback)
   EXPECT_NE(node->bias_curr_, bc);
 
   // Now send bias curr command
-  node->pc_bias_curr_response_future_ = node->send_pc_bias_curr_command(bc);
-  EXPECT_TRUE(node->pc_bias_curr_response_future_.valid()) << "Bias Current future invalid!";
-  node->pc_bias_curr_response_future_.wait();
+  PCROSNode::PCBiasCurrServiceResponseFuture pc_bias_curr_response_future =
+    node->send_pc_bias_curr_command(bc);
+  EXPECT_TRUE(pc_bias_curr_response_future.valid()) << "Bias Current future invalid!";
+  std::cout << "bias curr: before wait future" << std::endl;
+  pc_bias_curr_response_future.wait();
+  std::cout << "bias curr: after wait future" << std::endl;
   EXPECT_EQ(
-    node->pc_bias_curr_response_future_.get()->result.value,
-    node->pc_bias_curr_response_future_.get()->result.OK);
+    pc_bias_curr_response_future.get()->result.value,
+    pc_bias_curr_response_future.get()->result.OK);
 
   // Run a bit for bias curr command to move piston
   int bias_curr_iterations{9000}, bias_curr_timeout_iterations{10000};
+  std::cout << "bias curr: before server run" << std::endl;
   fixture->Server()->Run(true /*blocking*/, bias_curr_iterations, false /*paused*/);
+  std::cout << "bias curr: after server run" << std::endl;
   EXPECT_EQ(
     preCmdIterations + 2 * feedbackCheckIterations +
     torque_timeout_iterations + bias_curr_iterations,