Add new interface in cloisim_ros_micom

cloisim_ros_micom/README.md

@@ -25,3 +25,28 @@ ros2 run cloisim_ros_micom standalone --ros-args -p target_model:=cloi1 -p targe
 ```shell
 ros2 run cloisim_ros_micom standalone --ros-args -p enable_tf:=False -p target_model:=cloi1 -p target_parts_name:=RobotControl
 ```
+
+## Lawn mowing control
+
+### enable or disable mowing
+
+#### enable mowing
+
+```shell
+ros2 topic pub -1 /mowing/blade/rev_speed std_msgs/msg/UInt16 "{data: 1000}"
+```
+
+#### disable mowing
+
+```shell
+ros2 topic pub -1 /mowing/blade/rev_speed std_msgs/msg/UInt16 "{data: 0}"
+```
+
+### control height
+
+for example, adjust height of blade.
+
+```shell
+ros2 topic pub -1 /mowing/blade/height std_msgs/msg/Float32 "{data: 0.02}"
+ros2 topic pub -1 /mowing/blade/height std_msgs/msg/Float32 "{data: 0.09}"
+```

cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp

@@ -16,6 +16,7 @@
 #define CLOISIM_ROS_MICOM__MICOM_HPP_
 
 #include <cloisim_msgs/micom.pb.h>
+#include <cloisim_msgs/param.pb.h>
 
 #include <memory>
 #include <string>
@@ -25,6 +26,8 @@
 #include <sensor_msgs/msg/battery_state.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <std_srvs/srv/empty.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/u_int16.hpp>
 
 namespace cloisim_ros
 {
@@ -51,6 +54,10 @@ class Micom : public Base
 
   std::string MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) const;
 
+  std::string MakeMowingBladeHeightMessage(const std_msgs::msg::Float32::SharedPtr msg) const;
+
+  std::string MakeMowingRevSpeedMessage(const std_msgs::msg::UInt16::SharedPtr msg) const;
+
   void UpdateOdom();
   void UpdateImu();
   void UpdateBattery();
@@ -78,7 +85,13 @@ class Micom : public Base
   rclcpp::Publisher<sensor_msgs::msg::BatteryState>::SharedPtr pub_battery_;
 
   // wheel command subscriber
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_micom_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr sub_cmd_;
+
+  // set height of blade
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_blade_height_;
+
+  // set rev speed of blade
+  rclcpp::Subscription<std_msgs::msg::UInt16>::SharedPtr sub_blade_rev_speed_;
 
   // reset odometry pose service
   rclcpp::Service<std_srvs::srv::Empty>::SharedPtr srv_reset_odom_;

cloisim_ros_micom/src/micom.cpp

@@ -99,15 +99,33 @@ void Micom::Initialize()
     AddPublisherThread(tf_bridge_ptr, bind(&Base::GenerateTF, this, std::placeholders::_1));
   }
 
-  auto callback_sub = [this,
+  auto callback_sub_cmdvel = [this,
       data_bridge_ptr](const geometry_msgs::msg::Twist::SharedPtr msg) -> void {
       const auto msgBuf = MakeControlMessage(msg);
       SetBufferToSimulator(data_bridge_ptr, msgBuf);
     };
 
+  auto callback_sub_mowing_blade_height = [this,
+      data_bridge_ptr](const std_msgs::msg::Float32::SharedPtr msg) -> void {
+      const auto msgBuf = MakeMowingBladeHeightMessage(msg);
+      SetBufferToSimulator(data_bridge_ptr, msgBuf);
+    };
+
+  auto callback_sub_mowing_rev_speed = [this,
+      data_bridge_ptr](const std_msgs::msg::UInt16::SharedPtr msg) -> void {
+      const auto msgBuf = MakeMowingRevSpeedMessage(msg);
+      SetBufferToSimulator(data_bridge_ptr, msgBuf);
+    };
+
   // ROS2 Subscriber
-  sub_micom_ = create_subscription<geometry_msgs::msg::Twist>(
-    "cmd_vel", rclcpp::SensorDataQoS(), callback_sub);
+  sub_cmd_ = create_subscription<geometry_msgs::msg::Twist>(
+    "cmd_vel", rclcpp::SensorDataQoS(), callback_sub_cmdvel);
+
+  sub_blade_height_ = create_subscription<std_msgs::msg::Float32>(
+    "mowing/blade/height", rclcpp::ServicesQoS(), callback_sub_mowing_blade_height);
+
+  sub_blade_rev_speed_ = create_subscription<std_msgs::msg::UInt16>(
+    "mowing/blade/rev_speed", rclcpp::ServicesQoS(), callback_sub_mowing_rev_speed);
 
   srv_reset_odom_ = create_service<std_srvs::srv::Empty>(
     "reset_odometry", std::bind(&Micom::ResetOdometryCallback, this, _1, _2, _3));
@@ -146,6 +164,34 @@ string Micom::MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg)
   return message;
 }
 
+string Micom::MakeMowingBladeHeightMessage(const std_msgs::msg::Float32::SharedPtr msg) const
+{
+  cloisim::msgs::Param paramBuf;
+
+  paramBuf.set_name("mowing_blade_height");
+  auto pVal = paramBuf.mutable_value();
+  pVal->set_type(cloisim::msgs::Any::DOUBLE);
+  pVal->set_double_value(msg->data);
+
+  string message;
+  paramBuf.SerializeToString(&message);
+  return message;
+}
+
+string Micom::MakeMowingRevSpeedMessage(const std_msgs::msg::UInt16::SharedPtr msg) const
+{
+  cloisim::msgs::Param paramBuf;
+
+  paramBuf.set_name("mowing_blade_rev_speed");
+  auto pVal = paramBuf.mutable_value();
+  pVal->set_type(cloisim::msgs::Any::INT32);
+  pVal->set_int_value(msg->data);
+
+  string message;
+  paramBuf.SerializeToString(&message);
+  return message;
+}
+
 void Micom::PublishData(const string & buffer)
 {
   if (!pb_micom_.ParseFromString(buffer)) {
@@ -164,6 +210,7 @@ void Micom::PublishData(const string & buffer)
 
   // publish data
   PublishTF(odom_tf_);
+
   pub_odom_->publish(msg_odom_);
   pub_imu_->publish(msg_imu_);
   pub_battery_->publish(msg_battery_);