Using gimbal_2d Camera in ArduPilot Gazebo Simulator

Dear community,

I am seeking assistance with a problem regarding the control of the gimbal_2d camera in the ArduPilot Gazebo simulator.

My setup details are as follows:

I have a gimbal_2d camera, and there is a topic “/tilt_cmd” in the simulator (gazebo topic -t “/tilt_cmd”), but it is not visible in ROS. I aim to configure camera control through ROS, but I am encountering the issue of the topic not being visible in ROS.

I seek advice and assistance on the following questions:

  1. How can I make the “/tilt_cmd” topic visible in ROS?
  2. What is the correct procedure for configuring control of the gimbal_2d camera through ROS on the drone?

Additionally, I am interested in enabling zoom functionality for the camera. Could you please provide guidance on achieving this?

I would appreciate any support and recommendations. Thank you!

Hi @Jazx_Jazx , the topics in the documentation are only visible to the gz-transport messaging system and are not visible to ROS. If you want to use them from ROS then you will need to bridge the message types between ROS and Garbo using ros_gz. There should be support for simple data types but afaik this has not been tried.

Also I’d recommend following the guidance on the ArduPilot wiki for the new version of Gazebo as there have been many changes to the plugin library and the configuration is different to the version used with Gazebo 11

Support for camera zoom was added in this PR: Add camera zoom plugin by srmainwaring · Pull Request #57 · ArduPilot/ardupilot_gazebo · GitHub

1 Like

Thank you very much @ rhys for the answer!
Could you please tell me through which command to control directly, even from the gazebo topic? I try to send "gz topic -t “/gazebo/default/iris_demo/gimbal_tilt_cmd” -m gz.msgs.Double -p “data: -2” , but it doesn’t listen and Gimbal doesnt move

Names of Gimbal topics:
/gazebo/default/iris_demo/gimbal_tilt_cmd
/gazebo/default/iris_demo/gimbal_tilt_status

Plugin code

/*
 * Copyright (C) 2016 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/
#include <string>
#include <vector>

#include "gazebo/common/PID.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
#include "GimbalSmall2dPlugin.hh"

using namespace gazebo;
using namespace std;

GZ_REGISTER_MODEL_PLUGIN(GimbalSmall2dPlugin)

/// \brief Private data class
class gazebo::GimbalSmall2dPluginPrivate
{
  /// \brief Callback when a command string is received.
  /// \param[in] _msg Mesage containing the command string
  public: void OnStringMsg(ConstGzStringPtr &_msg);

  /// \brief A list of event connections
  public: std::vector<event::ConnectionPtr> connections;

  /// \brief Subscriber to the gimbal command topic
  public: transport::SubscriberPtr sub;

  /// \brief Publisher to the gimbal status topic
  public: transport::PublisherPtr pub;

  /// \brief Parent model of this plugin
  public: physics::ModelPtr model;

  /// \brief Joint for tilting the gimbal
  public: physics::JointPtr tiltJoint;

  /// \brief Command that updates the gimbal tilt angle
  public: double command = IGN_PI_2;

  /// \brief Pointer to the transport node
  public: transport::NodePtr node;

  /// \brief PID controller for the gimbal
  public: common::PID pid;

  /// \brief Last update sim time
  public: common::Time lastUpdateTime;
};

/////////////////////////////////////////////////
GimbalSmall2dPlugin::GimbalSmall2dPlugin()
  : dataPtr(new GimbalSmall2dPluginPrivate)
{
  this->dataPtr->pid.Init(1, 0, 0, 0, 0, 1.0, -1.0);
}

/////////////////////////////////////////////////
void GimbalSmall2dPlugin::Load(physics::ModelPtr _model,
  sdf::ElementPtr _sdf)
{
  this->dataPtr->model = _model;

  std::string jointName = "tilt_joint";
  if (_sdf->HasElement("joint"))
  {
    jointName = _sdf->Get<std::string>("joint");
  }
  this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(jointName);
  if (!this->dataPtr->tiltJoint)
  {
    std::string scopedJointName = _model->GetScopedName() + "::" + jointName;
    gzwarn << "joint [" << jointName
           << "] not found, trying again with scoped joint name ["
           << scopedJointName << "]\n";
    this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(scopedJointName);
  }
  if (!this->dataPtr->tiltJoint)
  {
    gzerr << "GimbalSmall2dPlugin::Load ERROR! Can't get joint '"
          << jointName << "' " << endl;
  }
}

/////////////////////////////////////////////////
void GimbalSmall2dPlugin::Init()
{
  this->dataPtr->node = transport::NodePtr(new transport::Node());
  this->dataPtr->node->Init(this->dataPtr->model->GetWorld()->GetName());

  this->dataPtr->lastUpdateTime =
    this->dataPtr->model->GetWorld()->GetSimTime();

  std::string topic = std::string("~/") +  this->dataPtr->model->GetName() +
    "/gimbal_tilt_cmd";
  this->dataPtr->sub = this->dataPtr->node->Subscribe(topic,
      &GimbalSmall2dPluginPrivate::OnStringMsg, this->dataPtr.get());

  this->dataPtr->connections.push_back(event::Events::ConnectWorldUpdateBegin(
          std::bind(&GimbalSmall2dPlugin::OnUpdate, this)));

  topic = std::string("~/") +
    this->dataPtr->model->GetName() + "/gimbal_tilt_status";

  this->dataPtr->pub =
    this->dataPtr->node->Advertise<gazebo::msgs::GzString>(topic);
}

/////////////////////////////////////////////////
void GimbalSmall2dPluginPrivate::OnStringMsg(ConstGzStringPtr &_msg)
{
  this->command = atof(_msg->data().c_str());
}

/////////////////////////////////////////////////
void GimbalSmall2dPlugin::OnUpdate()
{
  if (!this->dataPtr->tiltJoint)
    return;

  double angle = this->dataPtr->tiltJoint->GetAngle(0).Radian();

  common::Time time = this->dataPtr->model->GetWorld()->GetSimTime();
  if (time < this->dataPtr->lastUpdateTime)
  {
    this->dataPtr->lastUpdateTime = time;
    return;
  }
  else if (time > this->dataPtr->lastUpdateTime)
  {
    double dt = (this->dataPtr->lastUpdateTime - time).Double();
    double error = angle - this->dataPtr->command;
    double force = this->dataPtr->pid.Update(error, dt);
    this->dataPtr->tiltJoint->SetForce(0, force);
    this->dataPtr->lastUpdateTime = time;
  }

  static int i = 1000;
  if (++i > 100)
  {
    i = 0;
    std::stringstream ss;
    ss << angle;
    gazebo::msgs::GzString m;
    m.set_data(ss.str());
    this->dataPtr->pub->Publish(m);
  }
}

Hi @Jazx_Jazx - from the plugin code it looks like you are trying to run the old version of Gazebo (Gazebo 11 or Gazebo Classic as OSRF now refer to it: https://classic.gazebosim.org/).

The version of Gazebo that we support for ArduPilot is the new one: https://gazebosim.org/home, and we support both Gazebo Garden and Gazebo Harmonic.

With the new version of Gazebo there is no need for a separate plugin to control the gimbal because we use the joint position controllers provided in the Gazebo distribution. The example mentioned in the PR above shows how to set up a gimbal and command it.

1 Like