Skip to content

Commit

Permalink
fix action type casting
Browse files Browse the repository at this point in the history
  • Loading branch information
dirk-thomas committed Feb 15, 2019
1 parent a024084 commit a57d963
Showing 1 changed file with 8 additions and 5 deletions.
13 changes: 8 additions & 5 deletions rclcpp_action/include/rclcpp_action/server.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -337,8 +337,10 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
std::pair<GoalResponse, std::shared_ptr<void>>
call_handle_goal_callback(GoalID & uuid, std::shared_ptr<void> message) override
{
GoalResponse user_response = handle_goal_(
uuid, std::static_pointer_cast<typename ActionT::Goal>(message));
auto request = std::static_pointer_cast<
typename ActionT::Impl::SendGoalService::Request>(message);
auto goal = std::shared_ptr<typename ActionT::Goal>(request, &request->goal);
GoalResponse user_response = handle_goal_(uuid, goal);

auto ros_response = std::make_shared<typename ActionT::Impl::SendGoalService::Response>();
ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response ||
Expand Down Expand Up @@ -414,11 +416,12 @@ class Server : public ServerBase, public std::enable_shared_from_this<Server<Act
shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
};

auto request = std::static_pointer_cast<
const typename ActionT::Impl::SendGoalService::Request>(goal_request_message);
auto goal = std::shared_ptr<const typename ActionT::Goal>(request, &request->goal);
goal_handle.reset(
new ServerGoalHandle<ActionT>(
rcl_goal_handle, uuid,
std::static_pointer_cast<const typename ActionT::Goal>(goal_request_message),
on_terminal_state, on_executing, publish_feedback));
rcl_goal_handle, uuid, goal, on_terminal_state, on_executing, publish_feedback));
{
std::lock_guard<std::mutex> lock(goal_handles_mutex_);
goal_handles_[uuid] = goal_handle;
Expand Down

0 comments on commit a57d963

Please sign in to comment.