Merge pull request #84 from lge-ros2/main

Merge into jazzy

cloisim_ros_actor/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_actor</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>node for actor plugin</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_base/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_base</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>CLOiSim-ROS base class for other CLOiSim-ROS</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_bridge_zmq/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bridge_zmq</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>bridge for cloisim(simulator) connection through ZMQ</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_bringup/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bringup</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>

cloisim_ros_bringup_param/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_bringup_param</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>Bringup scripts and configurations for cloisim_ros</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_camera/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_camera</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>Virtual camera/depth camera/segmentation camera for CLOiSim</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_elevator_system/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_elevator_system</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>elevator system for simulation</description>
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

cloisim_ros_gps/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_gps</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>virtual gps for simulation</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_ground_truth/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_ground_truth</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>world plugin to retrieve ground truth</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_imu/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_imu</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>virtual imu for simulation</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_joint_control/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_joint_control</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>joint_control package for simulator</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_lidar/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_lidar</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>virtual lidar for simulation</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

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,9 +54,9 @@ class Micom : public Base
 
   std::string MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) const;
 
-  bool CalculateOdometry(
-    const rclcpp::Duration duration, const double _wheel_angular_vel_left,
-    const double _wheel_angular_vel_right, const double _theta);
+  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();
@@ -82,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/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_micom</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>micom package for simulator</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

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_);

cloisim_ros_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_msgs</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>interfaces package for cloisim_ros</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_multicamera/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_multicamera</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>virtual multi-camera for simulator</description>
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

cloisim_ros_protobuf_msgs/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_protobuf_msgs</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>CLOiSim-ROS interafces for communication between simulator and CLOiSim-ROS</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_realsense/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_realsense</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>virtual realsense for simulator</description>
   <maintainer email="[email protected]">Sungkyu Kang</maintainer>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>

cloisim_ros_sonar/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_sonar</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>virtual sonar for simulation</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_websocket_service/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_websocket_service</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>websocket service for cloisim(simulator) connection port control</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>

cloisim_ros_world/package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>cloisim_ros_world</name>
-  <version>4.3.2</version>
+  <version>4.4.0</version>
   <description>Utilities to interface with Unity through ROS.</description>
   <maintainer email="[email protected]">Hyunseok Yang</maintainer>
   <license>MIT</license>