Gimbal rotation control

Example

This section provides source code for controlling gimbal rotation and reading the gimbal angles. The source code is already available on the actual aircraft.

Build

cd /home/amov/p450_experiment/src/spirecv-ros/sv-rosapp/samples/control_basic/
mkdir build
cd build
cmake ..
make

Run

1. Start the appropriate gimbal driver for the aircraft model.

   P450-G1:

   /home/amov/p450_experiment/src/p450_experiment/scripts/gimbal_server_g1.sh
   

   The gimbal driver starts automatically after the ground control station connects to the UAV. In this case, you do not need to run the command above.

2. Run the demo.

   cd /home/amov/p450_experiment/src/spirecv-ros/sv-rosapp/samples/control_basic/build
   ./gimbal_control_by_angle
   

After execution, the gimbal rotates to roll, pitch, and yaw angles of 0°, 45°, and 0°, respectively, and prints the gimbal angles in the command line.

Program description

After the gimbal driver starts, this program communicates with the driver node through ROS topics, sends rotation commands, and obtains the gimbal angles. The driver node performs control, status reading, and other functions through the serial connection between the onboard computer and the gimbal.

[Publish] Gimbal control command

Topic: /uav1/gimbal/control (prometheus_msgs/GimbalControl)

Corresponding code:

const std::string gimbal_control_topic = "/uav1/gimbal/control";
control_pub_ = create_publisher<prometheus_msgs::msg::GimbalControl>(
  gimbal_control_topic, rclcpp::QoS(10));

prometheus_msgs::msg::GimbalControl msg;
msg.header.stamp = now();
msg.header.frame_id = "gimbal";
msg.mode = mode;
msg.angle[0] = angle[0];
msg.angle[1] = angle[1];
msg.angle[2] = angle[2];
msg.speed[0] = 0.0F;
msg.speed[1] = 0.0F;
msg.speed[2] = 0.0F;
control_pub_->publish(msg);
RCLCPP_INFO(this->get_logger(), "Command -> roll=%.1f pitch=%.1f yaw=%.1f",
  angle[0], angle[1], angle[2]);

[Subscribe] Gimbal status feedback

Topic: /uav1/gimbal/state (prometheus_msgs/GimbalState)

Corresponding code:

void onGimbalState(prometheus_msgs::msg::GimbalState::ConstSharedPtr msg)
{
  RCLCPP_INFO(this->get_logger(), "State: roll=%.2f pitch=%.2f yaw=%.2f",
    msg->angle_rt[0], msg->angle_rt[1], msg->angle_rt[2]);
}

const std::string gimbal_state_topic = "/uav1/gimbal/state";
state_sub_ = create_subscription<prometheus_msgs::msg::GimbalState>(
  gimbal_state_topic, rclcpp::QoS(10),
  std::bind(&GimbalControlByAngle::onGimbalState, this, std::placeholders::_1));

For more details, refer to:

/home/amov/p450_experiment/src/spirecv-ros/sv-rosapp/samples/control_basic/gimbal_control_by_angle.cpp

The source code contains detailed comments on publishing to the gimbal topic and obtaining status messages.