Skip to content
This repository has been archived by the owner on Oct 9, 2019. It is now read-only.

Fixing callback and result for dashing #93

Merged
merged 1 commit into from
Jun 4, 2019
Merged

Fixing callback and result for dashing #93

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
23 changes: 17 additions & 6 deletions individual_trajectories_bridge/src/FollowJointTrajectoryAction.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,11 +92,22 @@ void FollowJointTrajectoryAction::executeCB(const control_msgs::FollowJointTraje


printf("Sending goal\n");
std::function<void( rclcpp_action::ClientGoalHandle<hrim_actuator_rotaryservo_actions::action::GoalJointTrajectory>::SharedPtr,
const std::shared_ptr<const hrim_actuator_rotaryservo_actions::action::GoalJointTrajectory::Feedback> feedback )> cb_function = std::bind(
&FollowJointTrajectoryAction::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);

auto goal_handle_future = action_client->async_send_goal(goal_msg, cb_function);
// std::function<void( rclcpp_action::ClientGoalHandle<hrim_actuator_rotaryservo_actions::action::GoalJointTrajectory>::SharedPtr,
// const std::shared_ptr<const hrim_actuator_rotaryservo_actions::action::GoalJointTrajectory::Feedback> feedback )> cb_function = std::bind(
// &FollowJointTrajectoryAction::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
bool goal_response_received = false;
auto send_goal_ops = rclcpp_action::Client<hrim_actuator_rotaryservo_actions::action::GoalJointTrajectory>::SendGoalOptions();
send_goal_ops.goal_response_callback =
[&goal_response_received]
(std::shared_future<typename rclcpp_action::ClientGoalHandle<hrim_actuator_rotaryservo_actions::action::GoalJointTrajectory>::SharedPtr> future) mutable
{
auto goal_handle = future.get();
if (goal_handle) {
goal_response_received = true;
}
};

auto goal_handle_future = action_client->async_send_goal(goal_msg, send_goal_ops);

if (goal_handle_future.wait_for(std::chrono::seconds(5s)) != std::future_status::ready)
{
Expand All @@ -120,7 +131,7 @@ void FollowJointTrajectoryAction::executeCB(const control_msgs::FollowJointTraje
return;
}

rclcpp_action::ClientGoalHandle<hrim_actuator_rotaryservo_actions::action::GoalJointTrajectory>::Result result = result_future.get();
rclcpp_action::ClientGoalHandle<hrim_actuator_rotaryservo_actions::action::GoalJointTrajectory>::WrappedResult result = result_future.get();

switch(result.code) {
case rclcpp_action::ResultCode::SUCCEEDED:
Expand Down