joint_trajectoryの配信をactionからtopicに変更

sciurus17_examples/include/sciurus17_examples/neck_jt_control.hpp

@@ -16,12 +16,9 @@
 #define SCIURUS17_EXAMPLES__NECK_JT_CONTROL_HPP_
 
 #include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
 
-using GoalHandleJt = rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>;
-
 namespace sciurus17_examples
 {
 
@@ -31,12 +28,10 @@ class NeckJtControl : public rclcpp::Node
   explicit NeckJtControl(const rclcpp::NodeOptions & options);
 
 private:
-  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr client_ptr_;
   rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr angles_subscription_;
-  bool has_result_ = true;
+  rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr jt_publisher_;
 
   void angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg);
-  void result_callback(const GoalHandleJt::WrappedResult & result);
 };
 
 }  // namespace sciurus17_examples

sciurus17_examples/src/neck_jt_control.cpp

@@ -17,13 +17,11 @@
 #include "angles/angles.h"
 
 #include "rclcpp/rclcpp.hpp"
-#include "rclcpp_action/rclcpp_action.hpp"
-#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
 
 using std::placeholders::_1;
 using namespace std::chrono_literals;
-using GoalHandleJt = rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>;
 
 namespace sciurus17_examples
 {
@@ -34,14 +32,8 @@ NeckJtControl::NeckJtControl(const rclcpp::NodeOptions & options)
   angles_subscription_ = this->create_subscription<std_msgs::msg::Float64MultiArray>(
     "/target_angles", 10, std::bind(&NeckJtControl::angles_callback, this, _1));
 
-  this->client_ptr_ = rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(
-    this,
-    "neck_controller/follow_joint_trajectory");
-
-  if (!this->client_ptr_->wait_for_action_server(5s)) {
-    RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
-    rclcpp::shutdown();
-  }
+  jt_publisher_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>(
+    "neck_controller/joint_trajectory", 10);
 }
 
 void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
@@ -54,12 +46,6 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar
   const double MAX_PITCH_ANGLE = angles::from_degrees(50);
   const double MIN_PITCH_ANGLE = angles::from_degrees(-75);
 
-
-  // 動作完了していない場合はgoalを配信しない
-  if (!has_result_) {
-    return;
-  }
-
   // 角度指令値取得
   if (msg->data.size() != 2) {
     return;
@@ -72,45 +58,18 @@ void NeckJtControl::angles_callback(const std_msgs::msg::Float64MultiArray::Shar
   pitch_angle = std::clamp(pitch_angle, MIN_PITCH_ANGLE, MAX_PITCH_ANGLE);
 
   // joint名設定
-  auto goal_msg = control_msgs::action::FollowJointTrajectory::Goal();
-  goal_msg.trajectory.joint_names.push_back("neck_yaw_joint");
-  goal_msg.trajectory.joint_names.push_back("neck_pitch_joint");
+  trajectory_msgs::msg::JointTrajectory jt_msg;
+  jt_msg.joint_names.push_back("neck_yaw_joint");
+  jt_msg.joint_names.push_back("neck_pitch_joint");
 
   // 角度指令値設定
   trajectory_msgs::msg::JointTrajectoryPoint trajectory_point_msg;
   trajectory_point_msg.positions.push_back(yaw_angle);
   trajectory_point_msg.positions.push_back(pitch_angle);
   trajectory_point_msg.time_from_start = rclcpp::Duration::from_seconds(TIME_FROM_START);
-  goal_msg.trajectory.points.push_back(trajectory_point_msg);
-
-  auto send_goal_options =
-    rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
-  send_goal_options.result_callback = std::bind(&NeckJtControl::result_callback, this, _1);
-
-  // 角度指令値配信
-  this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
-  has_result_ = false;
-}
-
-void NeckJtControl::result_callback(
-  const GoalHandleJt::WrappedResult & result)
-{
-  switch (result.code) {
-    case rclcpp_action::ResultCode::SUCCEEDED:
-      break;
-    case rclcpp_action::ResultCode::ABORTED:
-      RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
-      return;
-    case rclcpp_action::ResultCode::CANCELED:
-      RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
-      return;
-    default:
-      RCLCPP_ERROR(this->get_logger(), "Unknown result code");
-      return;
-  }
+  jt_msg.points.push_back(trajectory_point_msg);
 
-  // 動作完了フラグをtrueにする
-  has_result_ = true;
+  jt_publisher_->publish(jt_msg);
 }
 
 }  // namespace sciurus17_examples

sciurus17_examples/src/object_tracker.cpp

@@ -31,7 +31,7 @@ ObjectTracker::ObjectTracker(const rclcpp::NodeOptions & options)
 : Node("object_tracker", options)
 {
   timer_ = this->create_wall_timer(
-    20ms, std::bind(&ObjectTracker::tracking, this));
+    10ms, std::bind(&ObjectTracker::tracking, this));
 
   state_subscription_ =
     this->create_subscription<control_msgs::msg::JointTrajectoryControllerState>(
@@ -64,7 +64,7 @@ void ObjectTracker::tracking()
   const std::vector<double> INITIAL_ANGLES = {0, 0};
 
   // 最大角度変化量
-  const double MAX_ANGULAR_DIFF = angles::from_degrees(1.0);
+  const double MAX_ANGULAR_DIFF = angles::from_degrees(0.5);
 
   // 物体が検出されなくなってから初期角度に戻り始めるまでの時間
   const std::chrono::nanoseconds DETECTION_TIMEOUT = 1s;