diff --git a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp index 4bf6f4d4..fdd94786 100644 --- a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp +++ b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -54,6 +55,8 @@ class Micom : public Base std::string MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) const; + std::string MakeControlMessage(const sensor_msgs::msg::Joy::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; @@ -87,6 +90,9 @@ class Micom : public Base // wheel command subscriber rclcpp::Subscription::SharedPtr sub_cmd_; + // joy command subscriber + rclcpp::Subscription::SharedPtr sub_joy_; + // set height of blade rclcpp::Subscription::SharedPtr sub_blade_height_; diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp index 998e75f4..f4a4856e 100644 --- a/cloisim_ros_micom/src/micom.cpp +++ b/cloisim_ros_micom/src/micom.cpp @@ -15,6 +15,7 @@ #include #include +#include #include #include @@ -105,6 +106,12 @@ void Micom::Initialize() SetBufferToSimulator(data_bridge_ptr, msgBuf); }; + auto callback_sub_joy = [this, + data_bridge_ptr](const sensor_msgs::msg::Joy::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); @@ -121,6 +128,9 @@ void Micom::Initialize() sub_cmd_ = create_subscription( "cmd_vel", rclcpp::SensorDataQoS(), callback_sub_cmdvel); + sub_joy_ = create_subscription( + "joy", rclcpp::SensorDataQoS(), callback_sub_joy); + sub_blade_height_ = create_subscription( "mowing/blade/height", rclcpp::ServicesQoS(), callback_sub_mowing_blade_height); @@ -164,6 +174,42 @@ string Micom::MakeControlMessage(const geometry_msgs::msg::Twist::SharedPtr msg) return message; } +string Micom::MakeControlMessage(const sensor_msgs::msg::Joy::SharedPtr msg) const +{ + cloisim::msgs::Joystick joyBuf; + + auto translation_ptr = joyBuf.mutable_translation(); + auto rotation_ptr = joyBuf.mutable_rotation(); + + // std::cout << "msg Axis="; + // for (const auto & msg_axis : msg->axes) std::cout << msg_axis << ", "; + // std::cout << std::endl; + const auto roll = msg->axes[0]; + const auto linear_x = msg->axes[1]; + const auto yaw = msg->axes[2]; // angular + const auto pitch = msg->axes[3]; + const auto linear_z = ((msg->axes[4] + 1.0) - (msg->axes[5] + 1.0)) * 0.5; + + translation_ptr->set_x(linear_x); + translation_ptr->set_y(0); + translation_ptr->set_z(linear_z); + rotation_ptr->set_x(roll); + rotation_ptr->set_y(pitch); + rotation_ptr->set_z(yaw); + + // std::cout << "msg Button="; + for (const auto& msg_button : msg->buttons) + { + joyBuf.add_buttons(msg_button); + // std::cout << msg_button << ", "; + } + // std::cout << std::endl; + + string message; + joyBuf.SerializeToString(&message); + return message; +} + string Micom::MakeMowingBladeHeightMessage(const std_msgs::msg::Float32::SharedPtr msg) const { cloisim::msgs::Param paramBuf; diff --git a/cloisim_ros_protobuf_msgs/CMakeLists.txt b/cloisim_ros_protobuf_msgs/CMakeLists.txt index de5a498f..7e7e522d 100644 --- a/cloisim_ros_protobuf_msgs/CMakeLists.txt +++ b/cloisim_ros_protobuf_msgs/CMakeLists.txt @@ -25,7 +25,7 @@ list(APPEND PROTOBUF_DEFINITION_FILES "laserscan;laserscan_stamped") list(APPEND PROTOBUF_DEFINITION_FILES "micom;battery;pointcloud;gps") list(APPEND PROTOBUF_DEFINITION_FILES "world_stats;log_playback_stats") list(APPEND PROTOBUF_DEFINITION_FILES "perception;perception_v") -list(APPEND PROTOBUF_DEFINITION_FILES "request;response") +list(APPEND PROTOBUF_DEFINITION_FILES "request;response;joystick") list(APPEND PROTOBUF_DEFINITION_FILES "joint_state;joint_state_v;joint_cmd;joint_cmd_v;pid") list(APPEND PROTOBUF_DEFINITION_FILES "transform_stamped")