There is no simple topic where you can publish a pose to reset the position and orientation of the quadrotor. You can call the /gazebo/set_model_state
service with the name of the quadrotor model to set a new pose, as with every model in Gazebo. The GazeboQuadrotorSimpleController plugin only accepts geometry_msgs/Twist inputs and cannot control the quadrotor's position.
However, the default quadrotor model does not use the GazeboQuadrotorSimpleController plugin, but the newer controller plugin based on ros_control (implemented in packages hector_quadrotor_controller and hector_quadrotor_controller_gazebo). This package also contains a pose controller which is not loaded by default. You can enable the pose controller by adding the string controller/pose to the list of controllers to be loaded in hector_quadrotor_controller/launch/controller.launch
:
<launch>
<rosparam file="$(find hector_quadrotor_controller)/params/controller.yaml" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
controller/pose controller/twist
"/>
</launch>
The pose controller subscribes to the command/pose
topic (geometry_msgs/PoseStamped), which is probably what you want.
The state and imu subscribers in GazeboQuadrotorSimpleController (the older plugin) and QuadrotorHardwareSim (used by the new ros_control plugin) have another purpose: If the state_topic
or imu_topic
parameters are non-empty, the controller plugin subscribes to these topics and uses them as the measured/estimated state to calculate the control outputs instead of the ground truth data as returned by the Gazebo API. For example, hector_quadrotor_pose_estimation estimates the pose and velocity from the simulated and noisy IMU, GPS, magnetometer and barometric pressure measurements, as a real onboard controller would do. The state_topic and imu_topic parameters are meant to feed these estimates back to the controller. If you would publish the desired poses there, this would render the quadrotor uncontrollable.
I'm afraid your question is a little confusing. Are you asking how to command the quadrotor in Gazebo? The way to command the quadrotor in Gazebo is to publish a geometymsgs/Twist messages on the /cmdvel topic, as is done in the teleop package.
Hello, the
/cmd_vel
isn't the only topic that the node subscribes to. There are others: Odometry (state_topic_) and IMU (imu_topic_). The state has pose information in it that is supposed to be read (Line 209), but it seems that pose is read from the imu topic (Line 252).Which particular node are you referring to?
The gazebo\_quadrotor\_simple\_controller.cpp from the link above.
If you are wanting to visualize the motion of a real physical quadrotor, I think you'll want to use Rviz instead of Gazebo. Gazebo is designed for simulating robots, where as Rviz is designed for visualizing real or simulated robots.
Thanks! I'll update the title of my question accordingly. Gazebo provides sensors (think augmented reality).
Is this question resolved?
Hi Nate, based on my reading of the GazeboQuadrotorSimpleController::StateCallback in gazebo_quadrotor_simple_controller.cpp, I think I'm right (i.e. publishing an odom message is all that's needed). But I was seeking clarification before wading into a new codebase. I should have time later on this week to confirm.