diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 87495e3f..bddcbf2e 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -8,10 +8,6 @@ "${workspaceFolder}/../../../install/perception_msgs/include/", "${workspaceFolder}/../../../install/elevator_system_msgs/include/", "${workspaceFolder}/../../../install/cloisim_ros_protobuf_msgs/include/", - "${workspaceFolder}/../../../install/cloisim_ros_msgs/include/", - "${workspaceFolder}/../../../install/perception_msgs/include/", - "${workspaceFolder}/../../../install/elevator_system_msgs/include/", - "${workspaceFolder}/../../../install/cloisim_ros_protobuf_msgs/include/", "${workspaceFolder}/../../../install/cloisim_ros_msgs/include/" ], "defines": [], diff --git a/cloisim_ros_micom/CMakeLists.txt b/cloisim_ros_micom/CMakeLists.txt index 0bbd7054..5add3c91 100644 --- a/cloisim_ros_micom/CMakeLists.txt +++ b/cloisim_ros_micom/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(std_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(cloisim_ros_base REQUIRED) find_package(cloisim_ros_bringup_param REQUIRED) @@ -22,6 +23,7 @@ set(dependencies sensor_msgs geometry_msgs nav_msgs + std_msgs std_srvs cloisim_ros_base cloisim_ros_bringup_param diff --git a/cloisim_ros_micom/README.md b/cloisim_ros_micom/README.md index b1272f90..7189144b 100644 --- a/cloisim_ros_micom/README.md +++ b/cloisim_ros_micom/README.md @@ -50,3 +50,13 @@ for example, adjust height of blade. 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}" ``` + +## Two-Wheel Self Balanced Robot(Q9) control + +### Joystick control + +Please refer to joy.config.yaml + +```shell +clear && ros2 launch teleop_twist_joy teleop-launch.py config_filepath:=./joy.config.yaml +``` diff --git a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp index fdd94786..4933d646 100644 --- a/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp +++ b/cloisim_ros_micom/include/cloisim_ros_micom/micom.hpp @@ -29,6 +29,8 @@ #include #include #include +#include +#include namespace cloisim_ros { @@ -64,6 +66,9 @@ class Micom : public Base void UpdateOdom(); void UpdateImu(); void UpdateBattery(); + void UpdateBumper(); + void UpdateIR(); + void UpdateUSS(); private: zmq::Bridge * info_bridge_ptr; @@ -82,10 +87,22 @@ class Micom : public Base // Battery sensor_msgs::msg::BatteryState msg_battery_; + // Bumper + std_msgs::msg::UInt8MultiArray msg_bumper_; + + // USS + std_msgs::msg::Float64MultiArray msg_uss_; + + // IR + std_msgs::msg::Float64MultiArray msg_ir_; + // ROS2 micom publisher rclcpp::Publisher::SharedPtr pub_imu_; rclcpp::Publisher::SharedPtr pub_odom_; rclcpp::Publisher::SharedPtr pub_battery_; + rclcpp::Publisher::SharedPtr pub_bumper_; + rclcpp::Publisher::SharedPtr pub_ir_; + rclcpp::Publisher::SharedPtr pub_uss_; // wheel command subscriber rclcpp::Subscription::SharedPtr sub_cmd_; diff --git a/cloisim_ros_micom/joy.config.yaml b/cloisim_ros_micom/joy.config.yaml new file mode 100644 index 00000000..beb61f84 --- /dev/null +++ b/cloisim_ros_micom/joy.config.yaml @@ -0,0 +1,18 @@ +teleop_twist_joy_node: + ros__parameters: + axis_linear: + x: 1 + scale_linear: + x: 0.5 + scale_linear_turbo: + x: 1.5 + + axis_angular: + yaw: 2 + scale_angular: + yaw: 0.8 + scale_angular_turbo: + yaw: 3.0 + + enable_button: 9 # L2 shoulder button + enable_turbo_button: 10 # L1 shoulder button \ No newline at end of file diff --git a/cloisim_ros_micom/src/micom.cpp b/cloisim_ros_micom/src/micom.cpp index f4a4856e..707efdcc 100644 --- a/cloisim_ros_micom/src/micom.cpp +++ b/cloisim_ros_micom/src/micom.cpp @@ -72,6 +72,15 @@ void Micom::Initialize() pub_battery_ = create_publisher("battery_state", rclcpp::SensorDataQoS()); + pub_bumper_ = + create_publisher("bumper", rclcpp::SensorDataQoS()); + + pub_ir_ = + create_publisher("ir", rclcpp::SensorDataQoS()); + + pub_uss_ = + create_publisher("uss", rclcpp::SensorDataQoS()); + { msg_odom_.header.frame_id = "odom"; msg_odom_.child_frame_id = "base_footprint"; @@ -198,8 +207,7 @@ string Micom::MakeControlMessage(const sensor_msgs::msg::Joy::SharedPtr msg) con rotation_ptr->set_z(yaw); // std::cout << "msg Button="; - for (const auto& msg_button : msg->buttons) - { + for (const auto & msg_button : msg->buttons) { joyBuf.add_buttons(msg_button); // std::cout << msg_button << ", "; } @@ -253,6 +261,9 @@ void Micom::PublishData(const string & buffer) UpdateOdom(); UpdateImu(); UpdateBattery(); + UpdateBumper(); + UpdateIR(); + UpdateUSS(); // publish data PublishTF(odom_tf_); @@ -260,6 +271,9 @@ void Micom::PublishData(const string & buffer) pub_odom_->publish(msg_odom_); pub_imu_->publish(msg_imu_); pub_battery_->publish(msg_battery_); + pub_bumper_->publish(msg_bumper_); + pub_uss_->publish(msg_uss_); + pub_ir_->publish(msg_ir_); } void Micom::UpdateOdom() @@ -324,4 +338,49 @@ void Micom::UpdateBattery() } } +void Micom::UpdateBumper() +{ + if (pb_micom_.has_bumper()) { + // std::cout << "bumper Size " << pb_micom_.bumper().bumped_size() << std::endl; + msg_bumper_.data.clear(); + msg_bumper_.data.resize(pb_micom_.bumper().bumped_size()); + + for (auto i = 0; i < pb_micom_.bumper().bumped_size(); i++) { + // std::cout << pb_micom_.bumper().bumped(i) << " "; + msg_bumper_.data[i] = pb_micom_.bumper().bumped(i); + } + // std::cout << std::endl; + } +} + +void Micom::UpdateIR() +{ + if (pb_micom_.has_ir()) { + // std::cout << "bumper Size " << pb_micom_.bumper().bumped_size() << std::endl; + msg_ir_.data.clear(); + msg_ir_.data.resize(pb_micom_.ir().distance_size()); + + for (auto i = 0; i < pb_micom_.ir().distance_size(); i++) { + // std::cout << pb_micom_.bumper().bumped(i) << " "; + msg_ir_.data[i] = pb_micom_.ir().distance(i); + } + // std::cout << std::endl; + } +} + +void Micom::UpdateUSS() +{ + if (pb_micom_.has_uss()) { + // std::cout << "bumper Size " << pb_micom_.bumper().bumped_size() << std::endl; + msg_uss_.data.clear(); + msg_uss_.data.resize(pb_micom_.uss().distance_size()); + + for (auto i = 0; i < pb_micom_.uss().distance_size(); i++) { + // std::cout << pb_micom_.bumper().bumped(i) << " "; + msg_uss_.data[i] = pb_micom_.uss().distance(i); + } + // std::cout << std::endl; + } +} + } // namespace cloisim_ros