Skip to content

Commit

Permalink
Add description for update functions
Browse files Browse the repository at this point in the history
  • Loading branch information
christophfroehlich committed Nov 26, 2023
1 parent c1a48df commit 7dbe314
Showing 1 changed file with 23 additions and 4 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,6 @@ const std::vector<double> INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0};
const std::vector<double> INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0};
const std::vector<double> INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0};

const auto update_rate = rclcpp::Duration::from_seconds(0.01);

bool is_same_sign_or_zero(double val1, double val2)
{
return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0);
Expand Down Expand Up @@ -413,7 +411,17 @@ class TrajectoryControllerTest : public ::testing::Test
trajectory_publisher_->publish(traj_msg);
}

void updateController(rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2))
/**
* @brief a wrapper for update() method of JTC, running synchronously with the clock
* @param wait_time - the time span for updating the controller
* @param update_rate - the rate at which the controller is updated
*
* @note use the faster updateControllerAsync() if no subscriptions etc.
* have to be used from the waitSet/executor
*/
void updateController(
rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2),
const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01))
{
auto clock = rclcpp::Clock(RCL_STEADY_TIME);
const auto start_time = clock.now();
Expand All @@ -429,9 +437,20 @@ class TrajectoryControllerTest : public ::testing::Test
}
}

/**
* @brief a wrapper for update() method of JTC, running asynchronously from the clock
* @return the time at which the update finished
* @param wait_time - the time span for updating the controller
* @param start_time - the time at which the update should start
* @param update_rate - the rate at which the controller is updated
*
* @note this is faster than updateController() and can be used if no subscriptions etc.
* have to be used from the waitSet/executor
*/
rclcpp::Time updateControllerAsync(
rclcpp::Duration wait_time = rclcpp::Duration::from_seconds(0.2),
rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME))
rclcpp::Time start_time = rclcpp::Time(0, 0, RCL_STEADY_TIME),
const rclcpp::Duration update_rate = rclcpp::Duration::from_seconds(0.01))
{
if (start_time == rclcpp::Time(0, 0, RCL_STEADY_TIME))
{
Expand Down

0 comments on commit 7dbe314

Please sign in to comment.