Programatically set the camera pose in gazebo or rviz.

edit

I tried approaching this issue in two ways: using the gazebo topics and a custom visual model plugin to move the camera pose programmatically.

1. Publishing to a Gazebo Camera Topic

These are the topics you need to publish for the former way to control position or velocity:

• /gazebo/default/user_camera/joy_pose
• /gazebo/default/user_camera/joy_twist

I have attached a demo script with which you can test out the functionality of joy_pose and joy_twist:

gz_camera_pub.cpp

#include <ros/ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <random>

#include <gazebo/msgs/msgs.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/gazebo_client.hh>


/**
* GZCameraPub Class
*/
class GZCameraPub {
private:
  float update_rate = 50.0;
  float time_period = 1. / update_rate;

  ros::Timer timer_;

  gazebo::transport::PublisherPtr gz_pose_pub_;
  gazebo::transport::PublisherPtr gz_twist_pub_;

public:
  /**
    * Constructor
    */
  GZCameraPub(ros::NodeHandle *nh, gazebo::transport::NodePtr gz_nh) {
    // define publishers
    gz_pose_pub_ = gz_nh->Advertise<gazebo::msgs::Pose>(
        "/gazebo/default/user_camera/joy_pose", 10);
    gz_twist_pub_ = gz_nh->Advertise<gazebo::msgs::Joystick>(
        "/gazebo/default/user_camera/joy_twist", 10);

    gz_pose_pub_->WaitForConnection();
    gz_twist_pub_->WaitForConnection();

    // Timers
    timer_ = nh->createTimer(
        ros::Duration(time_period), &GZCameraPub::timer_update, this);
  }

  void pubPose() {
    // Saved pose
    gazebo::msgs::Pose poseMsg;

    // use random
    std::random_device rd;
    std::mt19937 gen(rd());
    std::uniform_real_distribution<> dis(-5.0, 5.0);

    poseMsg.mutable_position()->set_x(dis(gen));
    poseMsg.mutable_position()->set_y(dis(gen));
    poseMsg.mutable_position()->set_z(dis(gen));

    tf2::Quaternion q;
    q.setRPY(0, 0, M_PI/2);
    poseMsg.mutable_orientation()->set_x(q.x());
    poseMsg.mutable_orientation()->set_y(q.y());
    poseMsg.mutable_orientation()->set_z(q.z());
    poseMsg.mutable_orientation()->set_w(q.w());
    gz_pose_pub_->Publish(poseMsg);
  }

  void pubVel() {
    gazebo::msgs::Joystick twistMsg;
    twistMsg.mutable_translation()->set_x(0);
    twistMsg.mutable_translation()->set_y(-0.8);
    twistMsg.mutable_translation()->set_z(0);

    twistMsg.mutable_rotation()->set_x(0);
    twistMsg.mutable_rotation()->set_y(0);
    twistMsg.mutable_rotation()->set_z(0);
    gz_twist_pub_->Publish(twistMsg);
  }

  /**
    * Function to test gazebo camera publishers
    */
  void timer_update(const ros::TimerEvent& event) {
    std::string mode = "pose";

    if (mode == "pose") {
      pubPose();
      ros::Duration(1.0).sleep();
    } else if (mode == "velocity") {
      pubVel();
    } else {
      ROS_ERROR("Invalid mode");
    }
  }
};

/**
* Main Function
*/
int main(int argc, char **argv) {
    // Initialize Gazebo client
    gazebo::client::setup(argc, argv);
    gazebo::transport::init();
    gazebo::transport::run();
    gazebo::transport::NodePtr gz_nh(new gazebo::transport::Node());
    gz_nh->Init("default");

    ros::init(argc, argv, "gz_camera_pub_node");
    ros::NodeHandle nh;

    GZCameraPub obj = GZCameraPub(&nh, gz_nh);
    ros::spin();

    gazebo::transport::fini();
    gazebo::client::shutdown();

    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(flappy_bird_gazebo)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  gazebo_msgs
)

find_package(gazebo REQUIRED)

catkin_package(
  CATKIN_DEPENDS roscpp gazebo_msgs
)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${GAZEBO_INCLUDE_DIRS}
)

link_directories(${GAZEBO_LIBRARY_DIRS})

add_executable(gz_camera_pub_node src/gz_camera_pub.cpp)
target_link_libraries(gz_camera_pub_node ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})

However, publishing to these topics over an extended field of time causes gazebo to lose all of its visuals. I did not know how to get around this, so I followed the second method.

2. Developing a Custom Visual Plugin.

This was relatively easier to implement, and the visual disappearing issue and the occasional gazebo freeze had disappeared.

custom.world

<model name="set_camera_pose_model">
  <link name="link">
    <visual name="visual">
      <geometry>
        <empty/>
      </geometry>
      <plugin name="CameraSim" filename="libCameraSimPlugin.so"/>
    </visual>
  </link>
</model>

Code snippet of the CameraSimPlugin class:

void CameraSimPlugin::Load(
  gazebo::rendering::VisualPtr visual, sdf::ElementPtr sdf) {
  // get ...