Skip to content

Commit

Permalink
コメント見て修正した
Browse files Browse the repository at this point in the history
  • Loading branch information
{Noir-swim} committed Jun 28, 2024
1 parent d81c8ad commit a3bfad1
Show file tree
Hide file tree
Showing 5 changed files with 14 additions and 62 deletions.
23 changes: 0 additions & 23 deletions packet/packet_composed/.vscode/c_cpp_properties.json

This file was deleted.

16 changes: 0 additions & 16 deletions packet/packet_composed/.vscode/settings.json

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -7,15 +7,13 @@

class Composed : public rclcpp::Node {
private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr _publisher;
rclcpp::Publisher<packet_interfaces::msg::Composed>::SharedPtr composed_publisher;

rclcpp::Subscription<packet_interfaces::msg::Depth>::SharedPtr depth_subscription;
rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription;
rclcpp::Subscription<packet_interfaces::msg::Flex>::SharedPtr flex1_subscription;
rclcpp::Subscription<packet_interfaces::msg::Flex>::SharedPtr flex2_subscription;
rclcpp::Subscription<packet_interfaces::msg::current>::SharedPtr current_subscription;
rclcpp::Subscription<packet_interfaces::msg::voltage>::SharedPtr voltage_subscription;
rclcpp::Subscription<packet_interfaces::msg::Current>::SharedPtr current_subscription;
rclcpp::Subscription<packet_interfaces::msg::Voltage>::SharedPtr voltage_subscription;

//まとめてpublishするためにタイマーのつもり
rclcpp::TimerBase::SharedPtr _timer;
Expand Down
6 changes: 0 additions & 6 deletions packet/packet_composed/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,6 @@
<depend>sensor_msgs</depend>
<depend>packet_interfaces</depend>
<depend>packet_interfaces</depend>

<exec_depend>rclcpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>packet_interfaces</exec_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
Expand Down
25 changes: 12 additions & 13 deletions packet/packet_composed/src/composed.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,26 +12,25 @@ Composed::Composed() :
{
//composed_publisherをコンストラクタで初期化した。
using namespace std::chrono_literals;
_publisher = this->create_publisher<std_msgs::msg::String>("/sensors_composed", 10);
composed_publisher = this->create_publisher<packet_interfaces::msg::Composed>("composed_topic", 10);

depth_subscription = this->create_subscription<packet_interfaces::msg::Depth>(
"depth_topic", 10, std::bind(&Composed::depth_topic_callback, this, std::placeholders::_1));
"sensors_depth", 10, std::bind(&Composed::depth_topic_callback, this, std::placeholders::_1));

imu_subscription = this->create_subscription<sensor_msgs::msg::Imu>(
"imu_topic", 10, std::bind(&Composed::imu_topic_callback, this, std::placeholders::_1));
"sensors_imu", 10, std::bind(&Composed::imu_topic_callback, this, std::placeholders::_1));

flex1_subscription = this->create_subscription<packet_interfaces::msg::Flex>(
"flex1_topic", 10, std::bind(&Composed::flex1_topic_callback, this, std::placeholders::_1));
"sensors_flex1", 10, std::bind(&Composed::flex1_topic_callback, this, std::placeholders::_1));

flex2_subscription = this->create_subscription<packet_interfaces::msg::Flex>(
"flex2_topic", 10, std::bind(&Composed::flex2_topic_callback, this, std::placeholders::_1));
"sensors_flex2", 10, std::bind(&Composed::flex2_topic_callback, this, std::placeholders::_1));

current_subscription = this->create_subscription<packet_interfaces::msg::Current>(
"current_topic", 10, std::bind(&Composed::current_topic_callback, this, std::placeholders::_1));
"sensors_current", 10, std::bind(&Composed::current_topic_callback, this, std::placeholders::_1));

voltage_subscription = this->create_subscription<packet_interfaces::msg::Voltage>(
"voltage_topic", 10, std::bind(&Composed::voltage_topic_callback, this, std::placeholders::_1));
"sensors_voltage", 10, std::bind(&Composed::voltage_topic_callback, this, std::placeholders::_1));

auto loop = std::bind(&Composed::_loop, this);
_timer = this->create_wall_timer(500ms, loop);
Expand All @@ -49,7 +48,7 @@ void Composed::_loop() {

void Composed::depth_topic_callback(const packet_interfaces::msg::Depth& msg)
{
RCLCPP_INFO(this->get_logger(), "Received Depth message");
RCLCPP_DEBUG(this->get_logger(), "Received Depth message");
// Composedメッセージにデータを追加
auto composed_msg = packet_interfaces::msg::Composed();
composed_msg.depth = msg;
Expand All @@ -59,7 +58,7 @@ void Composed::depth_topic_callback(const packet_interfaces::msg::Depth& msg)

void Composed::imu_topic_callback(const sensor_msgs::msg::Imu& msg)
{
RCLCPP_INFO(this->get_logger(), "Received IMU message");
RCLCPP_DEBUG(this->get_logger(), "Received IMU message");
// Composedメッセージにデータを追加
auto composed_msg = packet_interfaces::msg::Composed();
composed_msg.imu = msg;
Expand All @@ -69,7 +68,7 @@ void Composed::imu_topic_callback(const sensor_msgs::msg::Imu& msg)

void Composed::flex1_topic_callback(const packet_interfaces::msg::Flex1& msg)
{
RCLCPP_INFO(this->get_logger(), "Received Flex1 message");
RCLCPP_DEBUG(this->get_logger(), "Received Flex1 message");
// Composedメッセージにデータを追加
auto composed_msg = packet_interfaces::msg::Composed();
composed_msg.flex1 = msg;
Expand All @@ -79,7 +78,7 @@ void Composed::flex1_topic_callback(const packet_interfaces::msg::Flex1& msg)

void Composed::flex2_topic_callback(const packet_interfaces::msg::Flex2& msg)
{
RCLCPP_INFO(this->get_logger(), "Received Flex2 message");
RCLCPP_DEBUG(this->get_logger(), "Received Flex2 message");
// Composedメッセージにデータを追加
auto composed_msg = packet_interfaces::msg::Composed();
composed_msg.flex2 = msg;
Expand All @@ -89,7 +88,7 @@ void Composed::flex2_topic_callback(const packet_interfaces::msg::Flex2& msg)

void Composed::current_topic_callback(const packet_interfaces::msg::Current2& msg)
{
RCLCPP_INFO(this->get_logger(), "Received Current message");
RCLCPP_DEBUG(this->get_logger(), "Received Current message");
// Composedメッセージにデータを追加
auto composed_msg = packet_interfaces::msg::Composed();
composed_msg.Current = msg;
Expand All @@ -99,7 +98,7 @@ void Composed::current_topic_callback(const packet_interfaces::msg::Current2& ms

void Composed::voltage_topic_callback(const packet_interfaces::msg::Voltage& msg)
{
RCLCPP_INFO(this->get_logger(), "Received Flex2 message");
RCLCPP_DEBUG(this->get_logger(), "Received Flex2 message");
// Composedメッセージにデータを追加
auto composed_msg = packet_interfaces::msg::Composed();
composed_msg.voltage = msg;
Expand Down

0 comments on commit a3bfad1

Please sign in to comment.