Initial commit

.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

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,14 +36,12 @@ 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.
 
 ## Further work
 - Better dynamic reconfiguration
 - Better handling of gimbal timestamps
+- Publication of imu data and camera orientation

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})
 

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"))

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
+

gremsy_base/include/gremsy_base/ros_gremsy.h

@@ -13,6 +13,7 @@
 #include <boost/bind.hpp>
 #include "gimbal_interface.h"
 #include "serial_port.h"
+#include <signal.h>
 
 #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_;
+
+
 };

gremsy_base/package.xml

@@ -4,7 +4,7 @@
   <version>0.0.1</version>
   <description>The core of the ros_gremsy package</description>
 
-  <maintainer email="[email protected]">Florian Vahl</maintainer>
+  <maintainer email="[email protected]">Ivan Gutierrez</maintainer>
 
   <author email="[email protected]">Florian Vahl</author>
 

gremsy_base/src/ros_gremsy.cpp

@@ -1,5 +1,7 @@
 #include <gremsy_base/ros_gremsy.h>
 
+
+
 GimbalNode::GimbalNode(ros::NodeHandle nh, ros::NodeHandle pnh)
 {
     // Initialize dynamic-reconfigure
@@ -14,59 +16,49 @@ GimbalNode::GimbalNode(ros::NodeHandle nh, ros::NodeHandle pnh)
     goals_.vector.z = 0;
 
     // Advertive Publishers
-    imu_pub = nh.advertise<sensor_msgs::Imu>("/ros_gremsy/imu/data", 10);
     encoder_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/ros_gremsy/encoder", 1000);
-    mount_orientation_incl_global_yaw = nh.advertise<geometry_msgs::Quaternion>("/ros_gremsy/mount_orientation_global_yaw", 10);
-    mount_orientation_incl_local_yaw = nh.advertise<geometry_msgs::Quaternion>("/ros_gremsy/mount_orientation_local_yaw", 10);
 
 
     // Register Subscribers
     gimbal_goal_sub = nh.subscribe("/ros_gremsy/goals", 1, &GimbalNode::setGoalsCallback, this);
 
     // 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,119 +73,39 @@ 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<int16_t> 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)
 {
     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