Setpoint accelerations for Iris on ROS Gazebo not working (mavros)?

ROS 1 / ROS 2
1

I’m using an acceleration PD control for a quad simulation on ROS Gazebo using MAVROS PX4 SITL. I have the given the x and y accelerations in the feed-forward command and use the position and velocity for the feedback. The target is to reach a pre-defined setpoint. The quad just hovers at a fixed altitude and doesn’t reach the target position.

x_accel_y_accel_not_working
x_accel_y_accel_not_working1280×719 176 KB

Here is my code snippet of subscribed nodes and published nodes:

# Subscribe to drone's local position
rospy.Subscriber('mavros/local_position/pose', PoseStamped, cnt.posCb)

# Subscribe to drone's velocity
rospy.Subscriber("/mavros/local_position/velocity", TwistStamped , cnt.velCb) 

# Setpoint publisher
sp_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)`

The code gives the required acceleration setpoints to the quad as shown by

rostopic echo /mavros/setpoint_raw/local

`  ---
header: 
  seq: 1620
  stamp: 
  secs: 0
    nsecs:         0
  frame_id: ''
coordinate_frame: 1
type_mask: 1080
position: 
  x: 0.0
  y: 0.0
  z: 4.0
velocity: 
  x: 0.0
  y: 0.0
  z: 0.0
acceleration_or_force: 
  x: 1.60403175801
  y: 1.69204383269
  z: 0.0
yaw: 0.0
yaw_rate: 0.0`

I’ve attached the screenshot of the hovering drone.

2

@Jaroan I believe you don’t have the correct type mask. See

3

I have set all ignore flags except velocity flags and yaw rate to zero, as initially, the drone must reach the position of (0,0,4) and then take in the acceleration inputs.
My python class has this set for the type_mask
self.sp.type_mask = int('010000111000', 2)

4

@Jaroan If you enable the velocity setpoints, I think the acceleration setpoints will not be taken.

How are you making the transition between velocity setpoints and acceleration setpoints?

5

This is the brief outline of my code:

    # My desired waypoints are 
    x1,y1,z1 = 4,3,4
    while not cnt.state.armed:
            modes.setArm()
            rate.sleep()

    # few setpoint messages before offboard mode
    k=0
    while k<10:
            sp_pub.publish(cnt.sp) 
            rate.sleep()
            k = k + 1

    modes.setOffboardMode()

    # use a goto function that assigns the desired altitude to a variable of message type PositionTarget
    while not rospy.is_shutdown():
            cnt.goto(0,0,int(h) ,0)
            
            sp_pub.publish(cnt.sp)
            rate.sleep()

    # now once altitude reached, use acceleration setpoint function
    while not rospy.is_shutdown():

        #based on a PD controller class function, the required x and y accel are calculated and set in this loop until the error is small
         # self.sp.acceleration_or_force.x = self._accel_x
         # self.sp.acceleration_or_force.y = self._accel_y
         # publish into the topic 'mavros/setpoint_raw/local'
         sp_pub.publish(cnt.sp)
6

Update: I’ve tried three topics to publish the accelerations into:
/mavros/local_position/accel [geometry_msgs/AccelWithCovarianceStamped] , mavros/setpoint_accel/accel [geometry_msgs/Vector3Stamped] ,
/mavros/setpoint_raw/local [mavros_msgs/PositionTarget] (geometry_msgs/Vector3 acceleration_or_force)
The first and third caused no motion to the quad even though they published into the topics well (checked using ‘rostopic echo’). The second topic gave an initial acceleration in the x direction and quickly the drone crashed at a distance.

This is the brief outline of my first code:

    # My desired waypoints are 
    x1,y1,z1 = 4,3,4
    while not cnt.state.armed:
            modes.setArm()
            rate.sleep()

    # few setpoint messages before offboard mode
    k=0
    while k<10:
            sp_pub.publish(cnt.sp) 
            rate.sleep()
            k = k + 1

    modes.setOffboardMode()

    # use a goto function that assigns the desired altitude to a variable of message type PositionTarget
    while not rospy.is_shutdown():
            cnt.goto(0,0,int(h) ,0)
            
            sp_pub.publish(cnt.sp)
            rate.sleep()

    # now once altitude reached, use acceleration setpoint function
    while not rospy.is_shutdown():

        #based on a PD controller class function, the required x and y accel are calculated and set in this loop until the error is small
         # self.sp.acceleration_or_force.x = self._accel_x
         # self.sp.acceleration_or_force.y = self._accel_y
         # publish into the topic 'mavros/setpoint_raw/local'
         sp_pub.publish(cnt.sp)

@v01d I have seen a similar topic you faced difficulty in giving acceleration setpoints. Was it resolved? Trajectory Following via acceleration setpoints, which controller to use?
@Mzahana Trajectory Following via acceleration setpoints, which controller to use?