@@ -45,7 +45,7 @@ class FibonacciActionClient : public rclcpp::Node
4545
4646 this ->timer_ = this ->create_wall_timer (
4747 std::chrono::milliseconds (500 ),
48- std::bind (&FibonacciActionClient::send_goal, this ) );
48+ [ this ]() { return this -> send_goal ();} );
4949 }
5050
5151 ACTION_TUTORIALS_CPP_PUBLIC
@@ -67,67 +67,59 @@ class FibonacciActionClient : public rclcpp::Node
6767 RCLCPP_INFO (this ->get_logger (), " Sending goal" );
6868
6969 auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions ();
70- send_goal_options.goal_response_callback =
71- std::bind (&FibonacciActionClient::goal_response_callback, this , _1);
72- send_goal_options.feedback_callback =
73- std::bind (&FibonacciActionClient::feedback_callback, this , _1, _2);
74- send_goal_options.result_callback =
75- std::bind (&FibonacciActionClient::result_callback, this , _1);
70+ send_goal_options.goal_response_callback = [this ](
71+ const GoalHandleFibonacci::SharedPtr & goal_handle)
72+ {
73+ if (!goal_handle) {
74+ RCLCPP_ERROR (this ->get_logger (), " Goal was rejected by server" );
75+ } else {
76+ RCLCPP_INFO (this ->get_logger (), " Goal accepted by server, waiting for result" );
77+ }
78+ };
79+
80+ send_goal_options.feedback_callback = [this ](
81+ GoalHandleFibonacci::SharedPtr,
82+ const std::shared_ptr<const Fibonacci::Feedback> feedback)
83+ {
84+ std::stringstream ss;
85+ ss << " Next number in sequence received: " ;
86+ for (auto number : feedback->partial_sequence ) {
87+ ss << number << " " ;
88+ }
89+ RCLCPP_INFO (this ->get_logger (), ss.str ().c_str ());
90+ };
91+
92+ send_goal_options.result_callback = [this ](
93+ const GoalHandleFibonacci::WrappedResult & result)
94+ {
95+ switch (result.code ) {
96+ case rclcpp_action::ResultCode::SUCCEEDED:
97+ break ;
98+ case rclcpp_action::ResultCode::ABORTED:
99+ RCLCPP_ERROR (this ->get_logger (), " Goal was aborted" );
100+ return ;
101+ case rclcpp_action::ResultCode::CANCELED:
102+ RCLCPP_ERROR (this ->get_logger (), " Goal was canceled" );
103+ return ;
104+ default :
105+ RCLCPP_ERROR (this ->get_logger (), " Unknown result code" );
106+ return ;
107+ }
108+ std::stringstream ss;
109+ ss << " Result received: " ;
110+ for (auto number : result.result ->sequence ) {
111+ ss << number << " " ;
112+ }
113+ RCLCPP_INFO (this ->get_logger (), ss.str ().c_str ());
114+ rclcpp::shutdown ();
115+ };
116+
76117 this ->client_ptr_ ->async_send_goal (goal_msg, send_goal_options);
77118 }
78119
79120private:
80121 rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
81122 rclcpp::TimerBase::SharedPtr timer_;
82-
83- ACTION_TUTORIALS_CPP_LOCAL
84- void goal_response_callback (GoalHandleFibonacci::SharedPtr goal_handle)
85- {
86- if (!goal_handle) {
87- RCLCPP_ERROR (this ->get_logger (), " Goal was rejected by server" );
88- rclcpp::shutdown ();
89- } else {
90- RCLCPP_INFO (this ->get_logger (), " Goal accepted by server, waiting for result" );
91- }
92- }
93-
94- ACTION_TUTORIALS_CPP_LOCAL
95- void feedback_callback (
96- GoalHandleFibonacci::SharedPtr,
97- const std::shared_ptr<const Fibonacci::Feedback> feedback)
98- {
99- std::stringstream ss;
100- ss << " Next number in sequence received: " ;
101- for (auto number : feedback->partial_sequence ) {
102- ss << number << " " ;
103- }
104- RCLCPP_INFO (this ->get_logger (), " %s" , ss.str ().c_str ());
105- }
106-
107- ACTION_TUTORIALS_CPP_LOCAL
108- void result_callback (const GoalHandleFibonacci::WrappedResult & result)
109- {
110- switch (result.code ) {
111- case rclcpp_action::ResultCode::SUCCEEDED:
112- break ;
113- case rclcpp_action::ResultCode::ABORTED:
114- RCLCPP_ERROR (this ->get_logger (), " Goal was aborted" );
115- return ;
116- case rclcpp_action::ResultCode::CANCELED:
117- RCLCPP_ERROR (this ->get_logger (), " Goal was canceled" );
118- return ;
119- default :
120- RCLCPP_ERROR (this ->get_logger (), " Unknown result code" );
121- return ;
122- }
123- std::stringstream ss;
124- ss << " Result received: " ;
125- for (auto number : result.result ->sequence ) {
126- ss << number << " " ;
127- }
128- RCLCPP_INFO (this ->get_logger (), " %s" , ss.str ().c_str ());
129- rclcpp::shutdown ();
130- }
131123}; // class FibonacciActionClient
132124
133125} // namespace action_tutorials_cpp
0 commit comments