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.

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

RCLCPP Upgrade Bugfixes #1181

Merged
merged 4 commits into from
Apr 14, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ class MoveItControllerHandle
* Return true if the execution is complete (whether successful or not).
* Return false if timeout was reached.
* If timeout is -1 (default argument), wait until the execution is complete (no timeout). */
virtual bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration(-1)) = 0;
virtual bool waitForExecution(const rclcpp::Duration& timeout = rclcpp::Duration::from_nanoseconds(-1)) = 0;

/** \brief Return the execution status of the last trajectory sent to the controller. */
virtual ExecutionStatus getLastExecutionStatus() = 0;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -150,7 +150,7 @@ class GripperControllerHandle : public ActionBasedControllerHandle<control_msgs:
rclcpp_action::Client<control_msgs::action::GripperCommand>::SendGoalOptions send_goal_options;
// Active callback
send_goal_options.goal_response_callback =
[this](std::shared_future<rclcpp_action::Client<control_msgs::action::GripperCommand>::GoalHandle::SharedPtr>
[this](rclcpp_action::Client<control_msgs::action::GripperCommand>::GoalHandle::SharedPtr
/* unused-arg */) { RCLCPP_DEBUG_STREAM(LOGGER, name_ << " started execution"); };
// Send goal
auto current_goal_future = controller_action_client_->async_send_goal(goal, send_goal_options);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,11 +66,8 @@ bool FollowJointTrajectoryControllerHandle::sendTrajectory(const moveit_msgs::ms
rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions send_goal_options;
// Active callback
send_goal_options.goal_response_callback =
[this](
std::shared_future<rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::GoalHandle::SharedPtr>
future) {
[this](rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::GoalHandle::SharedPtr goal_handle) {
RCLCPP_INFO_STREAM(LOGGER, name_ << " started execution");
const auto& goal_handle = future.get();
if (!goal_handle)
RCLCPP_WARN(LOGGER, "Goal request rejected");
else
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -212,8 +212,7 @@ bool HybridPlanningManager::sendGlobalPlannerAction()

// Add goal response callback
global_goal_options.goal_response_callback =
[this](std::shared_future<rclcpp_action::ClientGoalHandle<moveit_msgs::action::GlobalPlanner>::SharedPtr> future) {
auto const& goal_handle = future.get();
[this](rclcpp_action::ClientGoalHandle<moveit_msgs::action::GlobalPlanner>::SharedPtr goal_handle) {
auto planning_progress = std::make_shared<moveit_msgs::action::HybridPlanner::Feedback>();
auto& feedback = planning_progress->feedback;
if (!goal_handle)
Expand Down Expand Up @@ -283,8 +282,7 @@ bool HybridPlanningManager::sendLocalPlannerAction()

// Add goal response callback
local_goal_options.goal_response_callback =
[this](std::shared_future<rclcpp_action::ClientGoalHandle<moveit_msgs::action::LocalPlanner>::SharedPtr> future) {
auto const& goal_handle = future.get();
[this](rclcpp_action::ClientGoalHandle<moveit_msgs::action::LocalPlanner>::SharedPtr goal_handle) {
auto planning_progress = std::make_shared<moveit_msgs::action::HybridPlanner::Feedback>();
auto& feedback = planning_progress->feedback;
if (!goal_handle)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -762,8 +762,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::MoveGroup>::SendGoalOptions();

send_goal_opts.goal_response_callback =
[&](std::shared_future<rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr> future) {
const auto& goal_handle = future.get();
[&](rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr goal_handle) {
if (!goal_handle)
{
done = true;
Expand Down Expand Up @@ -841,8 +840,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::MoveGroup>::SendGoalOptions();

send_goal_opts.goal_response_callback =
[&](std::shared_future<rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr> future) {
const auto& goal_handle = future.get();
[&](rclcpp_action::ClientGoalHandle<moveit_msgs::action::MoveGroup>::SharedPtr goal_handle) {
if (!goal_handle)
{
done = true;
Expand Down Expand Up @@ -906,9 +904,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
auto send_goal_opts = rclcpp_action::Client<moveit_msgs::action::ExecuteTrajectory>::SendGoalOptions();

send_goal_opts.goal_response_callback =
[&](std::shared_future<rclcpp_action::ClientGoalHandle<moveit_msgs::action::ExecuteTrajectory>::SharedPtr>
future) {
const auto& goal_handle = future.get();
[&](rclcpp_action::ClientGoalHandle<moveit_msgs::action::ExecuteTrajectory>::SharedPtr goal_handle) {
if (!goal_handle)
{
done = true;
Expand Down