Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

JTC touchups #abi-breaking #397

Merged
merged 6 commits into from
Jul 30, 2022
Merged

JTC touchups #abi-breaking #397

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
de867fe
Make topic_callback a member function instead of a lambda
bmagyar Jul 28, 2022
e34da39
Rename feedback_setup_callback to accepted_callback
bmagyar Jul 28, 2022
2612a71
remove comment
bmagyar Jul 29, 2022
9869d3a
Rename action server callbacks
bmagyar Jul 30, 2022
9883e54
fix function name
bmagyar Jul 30, 2022
072739d
reformat
bmagyar Jul 30, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 7 additions & 3 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -205,15 +205,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
rclcpp::TimerBase::SharedPtr goal_handle_timer_;
rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms);

// callback for topic interface
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void topic_callback(const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg);

// callbacks for action_server_
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
rclcpp_action::GoalResponse goal_callback(
rclcpp_action::GoalResponse goal_received_callback(
const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const FollowJTrajAction::Goal> goal);
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
rclcpp_action::CancelResponse cancel_callback(
rclcpp_action::CancelResponse goal_cancelled_callback(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
void feedback_setup_callback(
void goal_accepted_callback(
std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle);

// fill trajectory_msg so it matches joints controlled by this controller
Expand Down
52 changes: 23 additions & 29 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -697,30 +697,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
allow_integration_in_goal_trajectories_ =
get_node()->get_parameter("allow_integration_in_goal_trajectories").get_value<bool>();

// subscriber callback
// non realtime
// TODO(karsten): check if traj msg and point time are valid
auto callback = [this](const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg) -> void {
if (!validate_trajectory_msg(*msg))
{
return;
}

// http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement
// always replace old msg with new one for now
if (subscriber_is_active_)
{
add_new_trajectory_msg(msg);
}
};

// TODO(karsten1987): create subscriber with subscription deactivated
joint_command_subscriber_ =
get_node()->create_subscription<trajectory_msgs::msg::JointTrajectory>(
"~/joint_trajectory", rclcpp::SystemDefaultsQoS(), callback);

// TODO(karsten1987): no lifecycle for subscriber yet
// joint_command_subscriber_->on_activate();
"~/joint_trajectory", rclcpp::SystemDefaultsQoS(),
std::bind(&JointTrajectoryController::topic_callback, this, std::placeholders::_1));

// State publisher
RCLCPP_INFO(logger, "Controller state will be published at %.2f Hz.", state_publish_rate_);
Expand Down Expand Up @@ -774,9 +754,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
get_node()->get_node_base_interface(), get_node()->get_node_clock_interface(),
get_node()->get_node_logging_interface(), get_node()->get_node_waitables_interface(),
std::string(get_node()->get_name()) + "/follow_joint_trajectory",
std::bind(&JointTrajectoryController::goal_callback, this, _1, _2),
std::bind(&JointTrajectoryController::cancel_callback, this, _1),
std::bind(&JointTrajectoryController::feedback_setup_callback, this, _1));
std::bind(&JointTrajectoryController::goal_received_callback, this, _1, _2),
std::bind(&JointTrajectoryController::goal_cancelled_callback, this, _1),
std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1));

resize_joint_trajectory_point(state_current_, dof_);
resize_joint_trajectory_point(state_desired_, dof_);
Expand Down Expand Up @@ -857,7 +837,6 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
last_commanded_state_ = state;
}

// TODO(karsten1987): activate subscriptions of subscriber
return CallbackReturn::SUCCESS;
}

Expand Down Expand Up @@ -988,7 +967,22 @@ void JointTrajectoryController::publish_state(
}
}

rclcpp_action::GoalResponse JointTrajectoryController::goal_callback(
void JointTrajectoryController::topic_callback(
const std::shared_ptr<trajectory_msgs::msg::JointTrajectory> msg)
{
if (!validate_trajectory_msg(*msg))
{
return;
}
// http://wiki.ros.org/joint_trajectory_controller/UnderstandingTrajectoryReplacement
// always replace old msg with new one for now
if (subscriber_is_active_)
{
add_new_trajectory_msg(msg);
}
};

rclcpp_action::GoalResponse JointTrajectoryController::goal_received_callback(
const rclcpp_action::GoalUUID &, std::shared_ptr<const FollowJTrajAction::Goal> goal)
{
RCLCPP_INFO(get_node()->get_logger(), "Received new action goal");
Expand All @@ -1010,7 +1004,7 @@ rclcpp_action::GoalResponse JointTrajectoryController::goal_callback(
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
}

rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback(
rclcpp_action::CancelResponse JointTrajectoryController::goal_cancelled_callback(
const std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle)
{
RCLCPP_INFO(get_node()->get_logger(), "Got request to cancel goal");
Expand All @@ -1034,7 +1028,7 @@ rclcpp_action::CancelResponse JointTrajectoryController::cancel_callback(
return rclcpp_action::CancelResponse::ACCEPT;
}

void JointTrajectoryController::feedback_setup_callback(
void JointTrajectoryController::goal_accepted_callback(
std::shared_ptr<rclcpp_action::ServerGoalHandle<FollowJTrajAction>> goal_handle)
{
// Update new trajectory
Expand Down