Skip to content

Commit acee967

Browse files
authored
2.1.0リリースのためにmasterの変更差分をhumble-develへマージ (#46)
* object_trackingにおいて画像トピックをサブスクライブするように変更 (#43) * 画像トピックのサブスクライバを実装 * 画像トピックがRGBなのでBGRに変換 * mouseとcamera_nodeのオンオフを実装 * 実機で動作するように変更 * 物体追従のパラメータ調整 * 不要なメンバ変数を削除 * コールバック関数の名前を変更 * コメントアウトしていた不要な行を削除 * rclcpp::SensorDataQoS()を使用するように変更 * std::placeholders::_1を直接書くように修正 * raspimouseノードにComposableNodeを使用 * usb_camノードにもComposableNodeを使用 * 各ノードのuse_intra_process_commsをtrueに変更 * image_pub_を削除 * READMEのobject_trackingのコマンドを修正 * Gazeboの場合のコマンド例を削除 * READMEにGazeboでも実行できることを追記 (#44) * READMEにGazeboでも動作することを追記 * 英語版のREADMEにもGazeboの情報を追記 * READMEのサンプル集のリンクを修正 * READMEのraspimouse_simのリンクを修正 * リリースのためにCHANGELOG.rstとpackage.xmlを更新 (#45) * リリースのためにCHANGELOG.rstを更新 * 2.1.0
1 parent 5546840 commit acee967

File tree

8 files changed

+78
-47
lines changed

8 files changed

+78
-47
lines changed

CHANGELOG.rst

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,6 +2,12 @@
22
Changelog for package raspimouse_ros2_examples
33
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
44

5+
2.1.0 (2023-11-07)
6+
------------------
7+
* READMEにGazeboでも実行できることを追記 (`#44 <https://github.com/rt-net/raspimouse_ros2_examples/issues/44>`_)
8+
* object_trackingにおいて画像トピックをサブスクライブするように変更 (`#43 <https://github.com/rt-net/raspimouse_ros2_examples/issues/43>`_)
9+
* Contributors: YusukeKato
10+
511
2.0.0 (2023-08-03)
612
------------------
713
* Humble対応 (`#41 <https://github.com/rt-net/raspimouse_ros2_examples/issues/41>`_)

CMakeLists.txt

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ find_package(OpenCV REQUIRED)
3030
find_package(raspimouse_msgs REQUIRED)
3131
find_package(raspimouse REQUIRED)
3232
find_package(rt_usb_9axisimu_driver REQUIRED)
33+
find_package(cv_bridge REQUIRED)
3334

3435

3536
include_directories(include)
@@ -46,7 +47,8 @@ ament_target_dependencies(object_tracking_component
4647
std_srvs
4748
sensor_msgs
4849
geometry_msgs
49-
OpenCV)
50+
OpenCV
51+
cv_bridge)
5052
rclcpp_components_register_nodes(object_tracking_component "object_tracking::Tracker")
5153

5254
add_library(line_follower_component SHARED

README.en.md

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,9 @@
66

77
ROS 2 examples for Raspberry Pi Mouse.
88

9-
ROS1 examples is [here](https://github.com/rt-net/raspimouse_ros_examples).
9+
ROS1 examples is [here](https://github.com/rt-net/raspimouse_ros_examples/blob/master/README.en.md).
10+
11+
To run on Gazebo, click [here](https://github.com/rt-net/raspimouse_sim/blob/ros2/README.en.md).
1012

1113
<img src=https://rt-net.github.io/images/raspberry-pi-mouse/raspberry_pi_mouse.JPG width=500 />
1214

@@ -155,10 +157,10 @@ $ ./configure_camera.bash
155157
Then, launch nodes with the following command:
156158

157159
```sh
158-
$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py
160+
$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/dev/video0
159161
```
160162

161-
This sample publishes two topics: `raw_image` for the camera image and `result_image` for the object detection image.
163+
This sample publishes two topics: `camera/color/image_raw` for the camera image and `result_image` for the object detection image.
162164
These images can be viewed with [RViz](https://index.ros.org/r/rviz/)
163165
or [rqt_image_view](https://index.ros.org/doc/ros2/Tutorials/RQt-Overview-Usage/).
164166

README.md

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,9 @@
66

77
Raspberry Pi MouseのROS 2サンプルコード集です。
88

9-
ROS1のサンプルコード集は[こちら](https://github.com/rt-net/raspimouse_ros_examples)
9+
ROS1のサンプルコード集は[こちら](https://github.com/rt-net/raspimouse_ros_examples/blob/master/README.md)
10+
11+
Gazebo(シミュレータ)でも動作します。詳細は[こちら](https://github.com/rt-net/raspimouse_sim/blob/ros2/README.md)
1012

1113
<img src=https://rt-net.github.io/images/raspberry-pi-mouse/raspberry_pi_mouse.JPG width=500 />
1214

@@ -157,10 +159,10 @@ $ ./configure_camera.bash
157159
次のコマンドでノードを起動します。
158160

159161
```sh
160-
$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py
162+
$ ros2 launch raspimouse_ros2_examples object_tracking.launch.py video_device:=/dev/video0
161163
```
162164

163-
カメラ画像は`raw_image`、物体検出画像は`result_image`というトピックとして発行されます。
165+
カメラ画像は`camera/color/image_raw`、物体検出画像は`result_image`というトピックとして発行されます。
164166
これらの画像は[RViz](https://index.ros.org/r/rviz/)
165167
[rqt_image_view](https://index.ros.org/doc/ros2/Tutorials/RQt-Overview-Usage/)
166168
で表示できます。

include/raspimouse_ros2_examples/object_tracking_component.hpp

Lines changed: 2 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -38,23 +38,19 @@ class Tracker : public rclcpp_lifecycle::LifecycleNode
3838
explicit Tracker(const rclcpp::NodeOptions & options);
3939

4040
protected:
41-
void on_image_timer();
41+
void image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image);
4242
void on_cmd_vel_timer();
4343

4444
private:
4545
cv::VideoCapture cap_;
46-
int device_index_;
47-
double image_width_;
48-
double image_height_;
4946
bool object_is_detected_;
5047
cv::Point2d object_normalized_point_;
5148
double object_normalized_area_;
5249
geometry_msgs::msg::Twist cmd_vel_;
53-
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>> image_pub_;
5450
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::Image>> result_image_pub_;
5551
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>> cmd_vel_pub_;
5652
std::shared_ptr<rclcpp::Client<std_srvs::srv::SetBool>> motor_power_client_;
57-
rclcpp::TimerBase::SharedPtr image_timer_;
53+
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
5854
rclcpp::TimerBase::SharedPtr cmd_vel_timer_;
5955

6056
std::string mat_type2encoding(int mat_type);

launch/object_tracking.launch.py

Lines changed: 43 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -13,12 +13,31 @@
1313
# limitations under the License.
1414

1515
import launch
16+
from launch.actions import DeclareLaunchArgument
17+
from launch.substitutions import LaunchConfiguration
18+
from launch.conditions import IfCondition
1619
from launch_ros.actions import ComposableNodeContainer
1720
from launch_ros.actions import Node
1821
from launch_ros.descriptions import ComposableNode
1922

2023

2124
def generate_launch_description():
25+
declare_mouse = DeclareLaunchArgument(
26+
'mouse',
27+
default_value="true",
28+
description='Launch raspimouse node'
29+
)
30+
declare_use_camera_node = DeclareLaunchArgument(
31+
'use_camera_node',
32+
default_value='true',
33+
description='Use camera node.'
34+
)
35+
declare_video_device = DeclareLaunchArgument(
36+
'video_device',
37+
default_value='/dev/video0',
38+
description='Set video device.'
39+
)
40+
2241
"""Generate launch description with multiple components."""
2342
container = ComposableNodeContainer(
2443
name='object_tracking_container',
@@ -29,12 +48,27 @@ def generate_launch_description():
2948
ComposableNode(
3049
package='raspimouse_ros2_examples',
3150
plugin='object_tracking::Tracker',
32-
name='tracker'),
51+
name='tracker',
52+
extra_arguments=[{'use_intra_process_comms': True}]),
3353
ComposableNode(
3454
package='raspimouse',
3555
plugin='raspimouse::Raspimouse',
3656
name='raspimouse',
37-
parameters=[{'use_light_sensors': False}]),
57+
parameters=[{'use_light_sensors': False}],
58+
extra_arguments=[{'use_intra_process_comms': True}],
59+
condition=IfCondition(LaunchConfiguration('mouse'))),
60+
ComposableNode(
61+
package='usb_cam',
62+
plugin='usb_cam::UsbCamNode',
63+
name='usb_cam',
64+
remappings=[('image_raw', 'camera/color/image_raw')],
65+
parameters=[
66+
{'video_device': LaunchConfiguration('video_device')},
67+
{'frame_id': 'camera_color_optical_frame'},
68+
{'pixel_format': 'yuyv2rgb'}
69+
],
70+
extra_arguments=[{'use_intra_process_comms': True}],
71+
condition=IfCondition(LaunchConfiguration('use_camera_node'))),
3872
],
3973
output='screen',
4074
)
@@ -45,7 +79,12 @@ def generate_launch_description():
4579
executable='lifecycle_node_manager',
4680
output='screen',
4781
parameters=[{'components': ['raspimouse', 'tracker']}]
48-
4982
)
5083

51-
return launch.LaunchDescription([container, manager])
84+
return launch.LaunchDescription([
85+
declare_mouse,
86+
declare_use_camera_node,
87+
declare_video_device,
88+
container,
89+
manager
90+
])

package.xml

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>raspimouse_ros2_examples</name>
5-
<version>2.0.0</version>
5+
<version>2.1.0</version>
66
<description>Raspberry Pi Mouse examples</description>
77
<maintainer email="shop@rt-net.jp">RT Corporation</maintainer>
88

@@ -30,6 +30,7 @@
3030
<depend>hls_lfcd_lds_driver</depend>
3131
<depend>nav2_map_server</depend>
3232
<depend>slam_toolbox</depend>
33+
<depend>cv_bridge</depend>
3334

3435
<test_depend>ament_lint_auto</test_depend>
3536
<test_depend>ament_lint_common</test_depend>

src/object_tracking_component.cpp

Lines changed: 12 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include "rclcpp/rclcpp.hpp"
2626
#include "std_msgs/msg/string.hpp"
2727
#include "lifecycle_msgs/srv/change_state.hpp"
28+
#include "cv_bridge/cv_bridge.h"
2829

2930
using namespace std::chrono_literals;
3031
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
@@ -34,37 +35,34 @@ namespace object_tracking
3435

3536
Tracker::Tracker(const rclcpp::NodeOptions & options)
3637
: rclcpp_lifecycle::LifecycleNode("tracker", options),
37-
device_index_(0), image_width_(320), image_height_(240),
3838
object_is_detected_(false)
3939
{
4040
}
4141

42-
void Tracker::on_image_timer()
42+
void Tracker::image_callback(const sensor_msgs::msg::Image::SharedPtr msg_image)
4343
{
44+
auto cv_img = cv_bridge::toCvShare(msg_image, msg_image->encoding);
4445
auto msg = std::make_unique<sensor_msgs::msg::Image>();
4546
auto result_msg = std::make_unique<sensor_msgs::msg::Image>();
4647
msg->is_bigendian = false;
4748
result_msg->is_bigendian = false;
4849

4950
cv::Mat frame, result_frame;
50-
cap_ >> frame;
51+
cv::cvtColor(cv_img->image, frame, CV_RGB2BGR);
5152

5253
if (!frame.empty()) {
5354
tracking(frame, result_frame);
5455
convert_frame_to_message(result_frame, *result_msg);
5556
result_image_pub_->publish(std::move(result_msg));
56-
57-
convert_frame_to_message(frame, *msg);
58-
image_pub_->publish(std::move(msg));
5957
}
6058
}
6159

6260
void Tracker::on_cmd_vel_timer()
6361
{
6462
const double LINEAR_VEL = -0.5; // unit: m/s
6563
const double ANGULAR_VEL = -0.8; // unit: rad/s
66-
const double TARGET_AREA = 0.3; // 0.0 ~ 1.0
67-
const double OBJECT_AREA_THRESHOLD = 0.06; // 0.0 ~ 1.0
64+
const double TARGET_AREA = 0.1; // 0.0 ~ 1.0
65+
const double OBJECT_AREA_THRESHOLD = 0.01; // 0.0 ~ 1.0
6866

6967
// Detects an object and tracks it
7068
// when the number of pixels of the object is greater than the threshold.
@@ -117,7 +115,7 @@ void Tracker::tracking(const cv::Mat & input_frame, cv::Mat & result_frame)
117115
cv::Mat hsv;
118116
cv::cvtColor(input_frame, hsv, cv::COLOR_BGR2HSV);
119117
cv::Mat extracted_bin;
120-
cv::inRange(hsv, cv::Scalar(9, 100, 100), cv::Scalar(29, 255, 255), extracted_bin); // Orange
118+
cv::inRange(hsv, cv::Scalar(0, 100, 100), cv::Scalar(29, 255, 255), extracted_bin); // Red-Orange
121119
// cv::inRange(hsv, cv::Scalar(60, 100, 100), cv::Scalar(80, 255, 255), extracted_bin); // Green
122120
// cv::inRange(hsv, cv::Scalar(100, 100, 100), cv::Scalar(120, 255, 255), extracted_bin); // Blue
123121
input_frame.copyTo(result_frame, extracted_bin);
@@ -173,24 +171,15 @@ CallbackReturn Tracker::on_configure(const rclcpp_lifecycle::State &)
173171
{
174172
RCLCPP_INFO(this->get_logger(), "on_configure() is called.");
175173

176-
image_timer_ = create_wall_timer(100ms, std::bind(&Tracker::on_image_timer, this));
177174
cmd_vel_timer_ = create_wall_timer(50ms, std::bind(&Tracker::on_cmd_vel_timer, this));
178175
// Don't actually start publishing data until activated
179-
image_timer_->cancel();
180176
cmd_vel_timer_->cancel();
181177

182-
image_pub_ = create_publisher<sensor_msgs::msg::Image>("raw_image", 1);
183178
result_image_pub_ = create_publisher<sensor_msgs::msg::Image>("result_image", 1);
184179
cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
185-
186-
// Initialize OpenCV video capture stream.
187-
cap_.open(device_index_);
188-
cap_.set(cv::CAP_PROP_FRAME_WIDTH, image_width_);
189-
cap_.set(cv::CAP_PROP_FRAME_HEIGHT, image_height_);
190-
if (!cap_.isOpened()) {
191-
RCLCPP_ERROR(this->get_logger(), "Could not open video stream");
192-
return CallbackReturn::FAILURE;
193-
}
180+
image_sub_ = create_subscription<sensor_msgs::msg::Image>(
181+
"camera/color/image_raw", rclcpp::SensorDataQoS(),
182+
std::bind(&Tracker::image_callback, this, std::placeholders::_1));
194183

195184
return CallbackReturn::SUCCESS;
196185
}
@@ -210,10 +199,8 @@ CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &)
210199
request->data = true;
211200
auto future_result = motor_power_client_->async_send_request(request);
212201

213-
image_pub_->on_activate();
214202
result_image_pub_->on_activate();
215203
cmd_vel_pub_->on_activate();
216-
image_timer_->reset();
217204
cmd_vel_timer_->reset();
218205

219206
return CallbackReturn::SUCCESS;
@@ -222,10 +209,8 @@ CallbackReturn Tracker::on_activate(const rclcpp_lifecycle::State &)
222209
CallbackReturn Tracker::on_deactivate(const rclcpp_lifecycle::State &)
223210
{
224211
RCLCPP_INFO(this->get_logger(), "on_deactivate() is called.");
225-
image_pub_->on_deactivate();
226212
result_image_pub_->on_deactivate();
227213
cmd_vel_pub_->on_deactivate();
228-
image_timer_->cancel();
229214
cmd_vel_timer_->cancel();
230215

231216
object_is_detected_ = false;
@@ -238,11 +223,10 @@ CallbackReturn Tracker::on_cleanup(const rclcpp_lifecycle::State &)
238223
{
239224
RCLCPP_INFO(this->get_logger(), "on_cleanup() is called.");
240225

241-
image_pub_.reset();
242226
result_image_pub_.reset();
243227
cmd_vel_pub_.reset();
244-
image_timer_.reset();
245228
cmd_vel_timer_.reset();
229+
image_sub_.reset();
246230

247231
return CallbackReturn::SUCCESS;
248232
}
@@ -251,11 +235,10 @@ CallbackReturn Tracker::on_shutdown(const rclcpp_lifecycle::State &)
251235
{
252236
RCLCPP_INFO(this->get_logger(), "on_shutdown() is called.");
253237

254-
image_pub_.reset();
255238
result_image_pub_.reset();
256239
cmd_vel_pub_.reset();
257-
image_timer_.reset();
258240
cmd_vel_timer_.reset();
241+
image_sub_.reset();
259242

260243
return CallbackReturn::SUCCESS;
261244
}

0 commit comments

Comments
 (0)