Add velocity parameters for camera_line_follower (#50)

* Add velocity parameters for camera_line_follower * update parameter names * Update README.en.md Co-authored-by: YusukeKato <[email protected]> --------- Co-authored-by: YusukeKato <[email protected]>
rt-net · Feb 6, 2024 · 29ad351 · 29ad351
1 parent 85b3bf0
commit 29ad351
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 14 deletions.
diff --git a/README.en.md b/README.en.md
@@ -289,14 +289,22 @@ or [rqt_image_view](https://index.ros.org/p/rqt_image_view/).
 
 #### Parameters
 
-- `brightness_max_value`
+- `max_brightness`
   - Type: `int`
   - Default: 90
   - Maximum threshold value for image binarisation.
-- `brightness_min_value`
+- `min_brightness`
   - Type: `int`
   - Default: 0
   - Minimum threshold value for image binarisation.
+- `max_linear_vel`
+  - Type: `double`
+  - Default: 0.05
+  - Maximum linear velocity.
+- `max_angular_vel`
+  - Type: `double`
+  - Default: 0.8
+  - Maximum angular velocity.
 
 ```sh
 ros2 param set /camera_follower brightness_max_value 80

diff --git a/README.md b/README.md
@@ -293,14 +293,22 @@ $ ros2 launch raspimouse_ros2_examples camera_line_follower.launch.py video_devi
 
 #### Parameters
 
-- `brightness_max_value`
+- `max_brightness`
   - Type: `int`
   - Default: 90
   - 画像の2値化のしきい値の最大値
-- `brightness_min_value`
+- `min_brightness`
   - Type: `int`
   - Default: 0
   - 画像の2値化のしきい値の最小値
+- `max_linear_vel`
+  - Type: `double`
+  - Default: 0.05
+  - 直進速度の最大値
+- `max_angular_vel`
+  - Type: `double`
+  - Default: 0.8
+  - 旋回速度の最大値
 
 ```sh
 ros2 param set /camera_follower brightness_max_value 80

diff --git a/src/camera_line_follower_component.cpp b/src/camera_line_follower_component.cpp
@@ -28,8 +28,10 @@
 #include "lifecycle_msgs/srv/change_state.hpp"
 #include "cv_bridge/cv_bridge.h"
 
-constexpr auto BRIGHTNESS_MIN_VAL_PARAM = "brightness_min_value";
-constexpr auto BRIGHTNESS_MAX_VAL_PARAM = "brightness_max_value";
+constexpr auto MIN_BRIGHTNESS_PARAM = "min_brightness";
+constexpr auto MAX_BRIGHTNESS_PARAM = "max_brightness";
+constexpr auto LINEAR_VEL_PARAM = "max_linear_vel";
+constexpr auto ANGULAR_VEL_PARAM = "max_angular_vel";
 
 
 namespace camera_line_follower
@@ -71,16 +73,14 @@ void Camera_Follower::callback_switches(const raspimouse_msgs::msg::Switches::Sh
 
 void Camera_Follower::on_cmd_vel_timer()
 {
-  constexpr double LINEAR_VEL = 0.05;  // unit: m/s
-  constexpr double ANGULAR_VEL = -0.8;  // unit: rad/s
   constexpr double OBJECT_AREA_THRESHOLD = 0.01;  // 0.0 ~ 1.0
   geometry_msgs::msg::Twist cmd_vel;
 
   // Follow the line
   // when the number of pixels of the object is greater than the threshold.
   if (object_is_detected_ && object_normalized_area_ > OBJECT_AREA_THRESHOLD) {
-    cmd_vel.linear.x = LINEAR_VEL;
-    cmd_vel.angular.z = ANGULAR_VEL * object_normalized_point_.x;
+    cmd_vel.linear.x = get_parameter(LINEAR_VEL_PARAM).as_double();
+    cmd_vel.angular.z = -get_parameter(ANGULAR_VEL_PARAM).as_double() * object_normalized_point_.x;
   } else {
     cmd_vel.linear.x = 0.0;
     cmd_vel.angular.z = 0.0;
@@ -135,8 +135,8 @@ bool Camera_Follower::detect_line(const cv::Mat & input_frame, cv::Mat & result_
   cv::Mat extracted_bin;
   cv::inRange(
     gray,
-    get_parameter(BRIGHTNESS_MIN_VAL_PARAM).get_value<int>(),
-    get_parameter(BRIGHTNESS_MAX_VAL_PARAM).get_value<int>(),
+    get_parameter(MIN_BRIGHTNESS_PARAM).as_int(),
+    get_parameter(MAX_BRIGHTNESS_PARAM).as_int(),
     extracted_bin);
   input_frame.copyTo(result_frame, extracted_bin);
 
@@ -204,8 +204,10 @@ CallbackReturn Camera_Follower::on_configure(const rclcpp_lifecycle::State &)
     "switches", 1, std::bind(&Camera_Follower::callback_switches, this, std::placeholders::_1));
 
   // Set parameter defaults
-  declare_parameter(BRIGHTNESS_MIN_VAL_PARAM, 0);
-  declare_parameter(BRIGHTNESS_MAX_VAL_PARAM, 90);
+  declare_parameter(MIN_BRIGHTNESS_PARAM, 0);
+  declare_parameter(MAX_BRIGHTNESS_PARAM, 90);
+  declare_parameter(LINEAR_VEL_PARAM, 0.05);
+  declare_parameter(ANGULAR_VEL_PARAM, 0.8);
 
   return CallbackReturn::SUCCESS;
 }