From c7114fe2eeac1da563c267e057f0dbf47277ee6f Mon Sep 17 00:00:00 2001 From: ivangr3 Date: Mon, 26 Sep 2022 12:09:19 +0200 Subject: [PATCH] Initial commit --- .gitmodules | 4 + README.md | 14 +- gremsy_base/CMakeLists.txt | 6 +- gremsy_base/cfg/ROSGremsy.cfg | 8 -- gremsy_base/config/config.yaml | 8 +- gremsy_base/include/gremsy_base/ros_gremsy.h | 17 +-- gremsy_base/package.xml | 2 +- gremsy_base/src/gSDK | 1 + gremsy_base/src/gSDK_Linux | 1 - gremsy_base/src/ros_gremsy.cpp | 142 ++++--------------- 10 files changed, 46 insertions(+), 157 deletions(-) mode change 100755 => 100644 gremsy_base/cfg/ROSGremsy.cfg create mode 160000 gremsy_base/src/gSDK delete mode 160000 gremsy_base/src/gSDK_Linux diff --git a/.gitmodules b/.gitmodules index ba1734f..3be37e2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,7 @@ [submodule "gremsy_base/src/gSDK_Linux"] path = gremsy_base/src/gSDK_Linux url = ../../Gremsy/gSDK.git +[submodule "gremsy_base/src/gSDK"] + path = gremsy_base/src/gSDK + url = https://github.com/Gremsy/gSDK.git + branch = gSDK_V3_alpha diff --git a/README.md b/README.md index c7b897f..de8ed95 100644 --- a/README.md +++ b/README.md @@ -1,22 +1,20 @@ # ROS Gremsy Gimbal Interface -A ROS interface to control Gremsy gimbals. Based on the [gSDK](https://github.com/Gremsy/gSDK) interface and the MavLink protocol. +A ROS interface to control Gremsy V3 gimbals. Based on the [gSDK_V3_alpha](https://github.com/Gremsy/gSDK/tree/gSDK_V3_alpha) interface and the MavLink protocol. Disclaimer: This software package is not officially developed by or related to Gremsy. -![Build CI](https://github.com/Flova/ros_gremsy/workflows/Build%20CI/badge.svg) - ## Description -This package includes a ROS Node which warps the gSDK for the Gremsy Gimbals which are mainly used for physical image stabilization. +This package includes a ROS Node which warps the gSDK for the Gremsy V3 gimbals which are mainly used for physical image stabilization. The gimbal is connected via UART with a Linux host device running this node. Devices such as the Raspberry Pi feature a build-in UART interface others like most PCs or Laptops need a cheap USB Adapter. The used serial device, as well as many other gimbal specific parameters, can be configured in the `config.yaml` file. -The node publishes the gimbals encoder positions, imu measurements, and the camera mount orientation. +The node publishes the gimbals encoder positions. ## Setup Run the following commands to clone this repository and update all submodules (needed for the external gSDK repository). ``` -git clone --recurse-submodules https://github.com/Flova/ros_gremsy +git clone --recurse-submodules -b ros_gremsy_V3 https://github.com/Flova/ros_gremsy.git cd ros_gremsy ``` @@ -38,10 +36,7 @@ roslaunch gremsy_bringup gimbal.launch ## ROS Message API The node publishes: -- `/ros_gremsy/imu/data` with a [sensor_msgs/Imu](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/Imu.html) message containing the raw gyro and accelerometer values. - `/ros_gremsy/encoder` with a [geometry_msgs/Vector3Stamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Vector3Stamped.html) message containing the encode values around the x (roll), y (pitch) and z (yaw) axis. -- `/ros_gremsy/mount_orientation_global_yaw` with a [geometry_msgs/Quaternion](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Quaternion.html) message representing the camera mount orientation in the global frame. This measurement is imprecise in the yaw axis because of the gyro drift. -- `/ros_gremsy/mount_orientation_local_yaw` with a [geometry_msgs/Quaternion](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Quaternion.html) message representing the camera mount orientation in the global frame except for the yaw axis which is provided relative to the gimbals mount on the vehicle or robot. The node receives: - `/ros_gremsy/goals` expects a [geometry_msgs/Vector3Stamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Vector3Stamped.html) message containing the desired angles for each axis. The frame for each axis (local or global), as well as the stabilization mode, can be configured in the `config.yaml` file. @@ -49,3 +44,4 @@ The node receives: ## Further work - Better dynamic reconfiguration - Better handling of gimbal timestamps +- Publication of imu data and camera orientation diff --git a/gremsy_base/CMakeLists.txt b/gremsy_base/CMakeLists.txt index 9ea2412..8166a90 100644 --- a/gremsy_base/CMakeLists.txt +++ b/gremsy_base/CMakeLists.txt @@ -29,11 +29,11 @@ include_directories( ) include_directories( - src/gSDK_Linux/src/mavlink/include/mavlink/v2.0 - src/gSDK_Linux/src/ + src/gSDK/src/mavlink/include/mavlink/v2.0 + src/gSDK/src/ ) -set(SOURCES src/ros_gremsy.cpp src/gSDK_Linux/src/serial_port.cpp src/gSDK_Linux/src/gimbal_interface.cpp) +set(SOURCES src/ros_gremsy.cpp src/gSDK/src/serial_port.cpp src/gSDK/src/gimbal_interface.cpp src/gSDK/src/gimbal_protocol_v2.cpp src/gSDK/src/gimbal_protocol_v1.cpp src/gSDK/src/gimbal_protocol.cpp) add_executable(GimbalNode ${SOURCES}) diff --git a/gremsy_base/cfg/ROSGremsy.cfg b/gremsy_base/cfg/ROSGremsy.cfg old mode 100755 new mode 100644 index 45cb093..ee78ef6 --- a/gremsy_base/cfg/ROSGremsy.cfg +++ b/gremsy_base/cfg/ROSGremsy.cfg @@ -11,12 +11,4 @@ gen.add("state_poll_rate", double_t, 0, "Rate in which the gimbal data is polled gen.add("goal_push_rate", double_t, 0, "Rate in which the gimbal are pushed to the gimbal", min=0.0, max=300.0) gen.add("gimbal_mode", int_t, 0, "Control mode of the gimbal", min=0, max=2) -gen.add("tilt_axis_input_mode", int_t, 0, "Input mode of the gimbals tilt axis", min=0, max=2) -gen.add("tilt_axis_stabilize", bool_t, 0, "Input mode of the gimbals tilt axis", None) -gen.add("roll_axis_input_mode", int_t, 0, "Input mode of the gimbals tilt roll", min=0, max=2) -gen.add("roll_axis_stabilize", bool_t, 0, "Input mode of the gimbals tilt roll", None) -gen.add("pan_axis_input_mode", int_t, 0, "Input mode of the gimbals tilt pan", min=0, max=2) -gen.add("pan_axis_stabilize", bool_t, 0, "Input mode of the gimbals tilt pan", None) -gen.add("lock_yaw_to_vehicle", bool_t, 0, "Uses the yaw relative to the gimbal mount to prevent drift issues. Only a light stabilization is applied.", None) - exit(gen.generate("gremsy_base", "gremsy_base", "ROSGremsy")) diff --git a/gremsy_base/config/config.yaml b/gremsy_base/config/config.yaml index 83957c8..0564c18 100644 --- a/gremsy_base/config/config.yaml +++ b/gremsy_base/config/config.yaml @@ -4,10 +4,4 @@ baudrate: 115200 state_poll_rate: 10.0 goal_push_rate: 60.0 gimbal_mode: 1 -tilt_axis_input_mode: 2 -roll_axis_input_mode: 2 -pan_axis_input_mode: 2 -tilt_axis_stabilize: True -roll_axis_stabilize: True -pan_axis_stabilize: True -lock_yaw_to_vehicle: True + diff --git a/gremsy_base/include/gremsy_base/ros_gremsy.h b/gremsy_base/include/gremsy_base/ros_gremsy.h index efd6661..4886d82 100644 --- a/gremsy_base/include/gremsy_base/ros_gremsy.h +++ b/gremsy_base/include/gremsy_base/ros_gremsy.h @@ -13,6 +13,7 @@ #include #include "gimbal_interface.h" #include "serial_port.h" +#include #define DEG_TO_RAD (M_PI / 180.0) #define RAD_TO_DEG (180.0 / M_PI) @@ -31,13 +32,6 @@ class GimbalNode void gimbalGoalTimerCallback(const ros::TimerEvent& event); // Calback to set a new gimbal position void setGoalsCallback(geometry_msgs::Vector3Stamped message); - // Converts - sensor_msgs::Imu convertImuMavlinkMessageToROSMessage(mavlink_raw_imu_t message); - Eigen::Quaterniond convertYXZtoQuaternion(double roll, double pitch, double yaw); - // Maps integer mode - control_gimbal_axis_input_mode_t convertIntToAxisInputMode(int mode); - // Maps integer mode - control_gimbal_mode_t convertIntGimbalMode(int mode); // Gimbal SDK Gimbal_Interface* gimbal_interface_; @@ -46,15 +40,12 @@ class GimbalNode // Current config gremsy_base::ROSGremsyConfig config_; // Publishers - ros::Publisher - imu_pub, - encoder_pub, - mount_orientation_incl_global_yaw, - mount_orientation_incl_local_yaw; + ros::Publisher encoder_pub; // Subscribers ros::Subscriber gimbal_goal_sub; // Value store - double yaw_difference_ = 0; geometry_msgs::Vector3Stamped goals_; + + }; diff --git a/gremsy_base/package.xml b/gremsy_base/package.xml index c408b12..4eea60c 100644 --- a/gremsy_base/package.xml +++ b/gremsy_base/package.xml @@ -4,7 +4,7 @@ 0.0.1 The core of the ros_gremsy package - Florian Vahl + Ivan Gutierrez Florian Vahl diff --git a/gremsy_base/src/gSDK b/gremsy_base/src/gSDK new file mode 160000 index 0000000..fdc56b4 --- /dev/null +++ b/gremsy_base/src/gSDK @@ -0,0 +1 @@ +Subproject commit fdc56b48f7a23fb98865e9ddbb4f534bf3943a19 diff --git a/gremsy_base/src/gSDK_Linux b/gremsy_base/src/gSDK_Linux deleted file mode 160000 index ba5fa8f..0000000 --- a/gremsy_base/src/gSDK_Linux +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ba5fa8fa1e68d708a36301eb9253cb666d70bb10 diff --git a/gremsy_base/src/ros_gremsy.cpp b/gremsy_base/src/ros_gremsy.cpp index 95b1fcb..f9515db 100644 --- a/gremsy_base/src/ros_gremsy.cpp +++ b/gremsy_base/src/ros_gremsy.cpp @@ -1,5 +1,7 @@ #include + + GimbalNode::GimbalNode(ros::NodeHandle nh, ros::NodeHandle pnh) { // Initialize dynamic-reconfigure @@ -14,10 +16,7 @@ GimbalNode::GimbalNode(ros::NodeHandle nh, ros::NodeHandle pnh) goals_.vector.z = 0; // Advertive Publishers - imu_pub = nh.advertise("/ros_gremsy/imu/data", 10); encoder_pub = nh.advertise("/ros_gremsy/encoder", 1000); - mount_orientation_incl_global_yaw = nh.advertise("/ros_gremsy/mount_orientation_global_yaw", 10); - mount_orientation_incl_local_yaw = nh.advertise("/ros_gremsy/mount_orientation_local_yaw", 10); // Register Subscribers @@ -25,48 +24,41 @@ GimbalNode::GimbalNode(ros::NodeHandle nh, ros::NodeHandle pnh) // Define SDK objects serial_port_ = new Serial_Port(config_.device.c_str(), config_.baudrate); - gimbal_interface_ = new Gimbal_Interface(serial_port_); + gimbal_interface_ = new Gimbal_Interface(serial_port_, 1, MAV_COMP_ID_ONBOARD_COMPUTER, Gimbal_Interface::MAVLINK_GIMBAL_V1); // Start ther serial interface and the gimbal SDK serial_port_->start(); gimbal_interface_->start(); + // Wait for gimbal present + while (!gimbal_interface_->present()) { + ros::Duration(0.2).sleep(); + } + /////////////////// // Config Gimbal // /////////////////// // Check if gimbal is on - if(gimbal_interface_->get_gimbal_status().mode == GIMBAL_STATE_OFF) + if(gimbal_interface_->get_gimbal_status().mode == Gimbal_Interface::GIMBAL_STATE_OFF) { // Turn on gimbal ROS_INFO("TURN_ON!\n"); - gimbal_interface_->set_gimbal_motor_mode(TURN_ON); + gimbal_interface_->set_gimbal_motor(Gimbal_Interface::TURN_ON); } // Wait until the gimbal is on - while (gimbal_interface_->get_gimbal_status().mode < GIMBAL_STATE_ON) + while (gimbal_interface_->get_gimbal_status().mode < Gimbal_Interface::GIMBAL_STATE_ON) { ros::Duration(0.2).sleep(); } // Set gimbal control modes - - gimbal_interface_->set_gimbal_mode(convertIntGimbalMode(config_.gimbal_mode)); - - // Set modes for each axis - - control_gimbal_axis_mode_t tilt_axis_mode, roll_axis_mode, pan_axis_mode; - - tilt_axis_mode.input_mode = convertIntToAxisInputMode(config_.tilt_axis_input_mode); - tilt_axis_mode.stabilize = config_.tilt_axis_stabilize; - - roll_axis_mode.input_mode = convertIntToAxisInputMode(config_.roll_axis_input_mode); - roll_axis_mode.stabilize = config_.roll_axis_stabilize; - - pan_axis_mode.input_mode = convertIntToAxisInputMode(config_.pan_axis_input_mode); - pan_axis_mode.stabilize = config_.pan_axis_stabilize; - - gimbal_interface_->set_gimbal_axes_mode(tilt_axis_mode, roll_axis_mode, pan_axis_mode); + if(config_.gimbal_mode == 1){ + gimbal_interface_->set_gimbal_follow_mode_sync(); + } else { + gimbal_interface_->set_gimbal_lock_mode_sync(); + } ros::Timer poll_timer = nh.createTimer( ros::Duration(1/config_.state_poll_rate), @@ -81,67 +73,26 @@ GimbalNode::GimbalNode(ros::NodeHandle nh, ros::NodeHandle pnh) void GimbalNode::gimbalStateTimerCallback(const ros::TimerEvent& event) { - // Publish Gimbal IMU - mavlink_raw_imu_t imu_mav = gimbal_interface_->get_gimbal_raw_imu(); - imu_mav.time_usec = gimbal_interface_->get_gimbal_time_stamps().raw_imu; // TODO implement rostime - sensor_msgs::Imu imu_ros_mag = convertImuMavlinkMessageToROSMessage(imu_mav); - imu_pub.publish(imu_ros_mag); - // Publish Gimbal Encoder Values - mavlink_mount_status_t mount_status = gimbal_interface_->get_gimbal_mount_status(); + attitude encoder_values = gimbal_interface_->get_gimbal_encoder(); + geometry_msgs::Vector3Stamped encoder_ros_msg; encoder_ros_msg.header.stamp = ros::Time::now(); - encoder_ros_msg.vector.x = ((float) mount_status.pointing_b) * DEG_TO_RAD; - encoder_ros_msg.vector.y = ((float) mount_status.pointing_a) * DEG_TO_RAD; - encoder_ros_msg.vector.z = ((float) mount_status.pointing_c) * DEG_TO_RAD; - // encoder_ros_msg.header TODO time stamps - - encoder_pub.publish(encoder_ros_msg); - - // Get Mount Orientation - mavlink_mount_orientation_t mount_orientation = gimbal_interface_->get_gimbal_mount_orientation(); - - yaw_difference_ = DEG_TO_RAD * (mount_orientation.yaw_absolute - mount_orientation.yaw); - - // Publish Camera Mount Orientation in global frame (drifting) - mount_orientation_incl_global_yaw.publish( - tf2::toMsg( - convertYXZtoQuaternion( - mount_orientation.roll, - mount_orientation.pitch, - mount_orientation.yaw_absolute))); - - // Publish Camera Mount Orientation in local frame (yaw relative to vehicle) - mount_orientation_incl_local_yaw.publish( - tf2::toMsg( - convertYXZtoQuaternion( - mount_orientation.roll, - mount_orientation.pitch, - mount_orientation.yaw))); -} + encoder_ros_msg.vector.x = ((float) encoder_values.roll); + encoder_ros_msg.vector.y = ((float) encoder_values.pitch); + encoder_ros_msg.vector.z = ((float) encoder_values.yaw); -Eigen::Quaterniond GimbalNode::convertYXZtoQuaternion(double roll, double pitch, double yaw) -{ - Eigen::Quaterniond quat_abs( - Eigen::AngleAxisd(-DEG_TO_RAD * pitch, Eigen::Vector3d::UnitY()) - * Eigen::AngleAxisd(-DEG_TO_RAD * roll, Eigen::Vector3d::UnitX()) - * Eigen::AngleAxisd(DEG_TO_RAD * yaw, Eigen::Vector3d::UnitZ())); - return quat_abs; + encoder_pub.publish(encoder_ros_msg); } + + + void GimbalNode::gimbalGoalTimerCallback(const ros::TimerEvent& event) { - double z = goals_.vector.z; - if (config_.lock_yaw_to_vehicle) - { - z += yaw_difference_; - } + gimbal_interface_->set_gimbal_rotation_sync(goals_.vector.y,goals_.vector.x, goals_.vector.z); - gimbal_interface_->set_gimbal_move( - RAD_TO_DEG * goals_.vector.y, - RAD_TO_DEG * goals_.vector.x, - RAD_TO_DEG * z); } void GimbalNode::setGoalsCallback(geometry_msgs::Vector3Stamped message) @@ -149,51 +100,12 @@ void GimbalNode::setGoalsCallback(geometry_msgs::Vector3Stamped message) goals_ = message; } -sensor_msgs::Imu GimbalNode::convertImuMavlinkMessageToROSMessage(mavlink_raw_imu_t message) -{ - sensor_msgs::Imu imu_message; - - // Set accelaration data - imu_message.linear_acceleration.x = message.xacc; - imu_message.linear_acceleration.y = message.yacc; - imu_message.linear_acceleration.z = message.zacc; - - // Set gyro data - imu_message.angular_velocity.x = message.xgyro; - imu_message.angular_velocity.y = message.ygyro; - imu_message.angular_velocity.z = message.zgyro; - - return imu_message; -} - -control_gimbal_mode_t GimbalNode::convertIntGimbalMode(int mode) -{ // Allows int access to the control_gimbal_mode_t struct - switch(mode) { - case 0 : return GIMBAL_OFF; - case 1 : return LOCK_MODE; - case 2 : return FOLLOW_MODE; - default: - ROS_ERROR_ONCE("Undefined gimbal mode used. Check the config file."); - return GIMBAL_OFF; - } -} - -control_gimbal_axis_input_mode_t GimbalNode::convertIntToAxisInputMode(int mode) -{ // Allows int access to the control_gimbal_axis_input_mode_t struct - switch(mode) { - case 0 : return CTRL_ANGLE_BODY_FRAME; - case 1 : return CTRL_ANGULAR_RATE; - case 2 : return CTRL_ANGLE_ABSOLUTE_FRAME; - default: - ROS_ERROR_ONCE("Undefined axis input mode used. Check the config file."); - return CTRL_ANGLE_ABSOLUTE_FRAME; - } -} void GimbalNode::reconfigureCallback(gremsy_base::ROSGremsyConfig &config, uint32_t level) { config_ = config; } + int main(int argc, char **argv) { // Init