Setpoint_Position/Local Position Control: Relative Positioning

SchaeferB1 commented 5 years ago

Issue details

Hello Everyone, I am attempting to control a quadcopter‘s position using a camera to move to a target using the difference between the current location of the drone and the centroid of a target. To do this, I am using the current position from ~local_position/pose, and subtracting a value from the current position that would then be published to ~setpoint_position/local.
The problem is the position value from ~local_position/pose resets to approximately (0,0,0) in (x,y,z) when running the script every time, making the ~local_position/pose values reset. After running the script once, the position from ~local_position/pose seems to no longer be in the same frame. When publishing to the ~setpoint_position/local topic, coordinates are published in a coordinate frame with an origin centered at the location of where the flight controller was booted. Although both topics use the ENU coordinate frame, the origin of the ~local_position/pose position is changed every time the script is run. Is there any possible way to get local position data that does not reset every time a new script is run? The posted script replaces the camera data that would be used in lines 146-147 with fixed constant values for testing. When the script is run, the drone moves to (5m,5m,3m) and lands. When the script is run again, it takes off and remains in the same (x,y) position. Other posts suggest changing the apm_config.yaml. Does changing the frame of ~setpoint_position/local in this configuration file change the frame that the input values are converted to?

Code

#!/usr/bin/env python
# Importing Libraries
import mavros
import rospy
import numpy as np
import math
import mavros_msgs
import time
from mavros_msgs import srv
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import SetModeResponse
from mavros_msgs.srv import CommandBool
from mavros_msgs.srv import CommandTOL
from mavros_msgs.srv import ParamSet
from mavros_msgs.msg import ParamValue
from mavros_msgs.msg import State
from mavros_msgs.msg import OverrideRCIn
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped
from sensor_msgs.msg import NavSatFix
from tf.transformations import quaternion_from_euler
from std_msgs.msg import String

alt_set = 2
class Operation(): 
    # Staring class Operation 
    goal_pose = PoseStamped() # renaming PoseStamped presumably end position 
    set_velocity = TwistStamped() # renaming TwistStamped to set velocity to be used as a structure
    current_state = State() # renaming state which relays if FCU is connected to GCU, armed, and guided

    def state_callback(self,data):
        self.cur_state = data
        # Gets state from /mavros/state

    def pose_sub_callback(self,pose_sub_data):
        self.current_pose = pose_sub_data
        # Callback function to get current position for FCU   

    def gps_callback(self,data):
        self.gps = data
        self.gps_read = True
        # Gets GPS data and will set a bool value for read

    def start_operation(self):
        rospy.init_node('my_operation', anonymous=True) # Starting node for operation 
        self.gps_read = False # When starting gps read will be sent to false initially
        self.localtarget_received = False 
        r = rospy.Rate(10) # Sents rate to 10Hz
        rospy.Subscriber("/mavros/state", State, self.state_callback) # Pulling information for mavros/state and sending it to state callback
        rospy.Subscriber("/mavros/global_position/global", NavSatFix, self.gps_callback) # Pulling GPS information and will not access callback to make gps_read statement true if NavSatFix = -1  
        ##rospy.Subscriber("cv_node",CV,self.cv_callback)
        local_position_subscribe = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.pose_sub_callback) # Getting current position and sets current_pose to subscribed value
        local_position_pub = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size = 10) 
        setpoint_velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',TwistStamped, queue_size = 10)
        while not self.gps_read:
            r.sleep()

        # Service Clients
        change_mode = rospy.ServiceProxy('/mavros/set_mode', SetMode) 
        arm = rospy.ServiceProxy('/mavros/cmd/arming', CommandBool)
        takeoff = rospy.ServiceProxy('/mavros/cmd/takeoff', CommandTOL)
        change_param = rospy.ServiceProxy('/mavros/param/set', ParamSet)
        last_request = rospy.get_rostime()

        # Change mode to GUIDED
        rospy.wait_for_service('/mavros/set_mode')
        try:
            base_mode = 0
            custom_mode = "GUIDED"
            out = change_mode(base_mode, custom_mode)
            print(out)
            if out:
                rospy.loginfo("GUIDED mode set")
            else:
                rospy.loginfo("Failed SetMode")
        except rospy.ServiceException, e:
            rospy.loginfo("Service call failed")
        last_request = rospy.get_rostime() 

        while not out:
            r.sleep()
            out = change_mode(base_mode, custom_mode)
            if out:
                rospy.loginfo("setmode send ok value")
            else:
                rospy.loginfo("Failed SetMode")

        # Arm drone
        rospy.wait_for_service('/mavros/cmd/arming')
        try:
            out = arm(True)
            if out:
                rospy.loginfo("Armed")
            else:
                rospy.loginfo("Failed Arming")
        except rospy.ServiceException, e:
            rospy.loginfo("Service call failed")
        last_request = rospy.get_rostime() 

        while not out:
            r.sleep()
            out = arm(True)
            if out:
                rospy.loginfo("Armed")
            else:
                rospy.loginfo("Failed Arming")

        # Take off
        current_altitude = self.gps.altitude 
        print('\n Takeoff')
        rospy.wait_for_service('/mavros/cmd/takeoff')
        try:        
            min_pitch = 0
            yaw = 0
            latitude = self.gps.latitude
            longitude = self.gps.longitude
            altitude = alt_set
            out = takeoff(min_pitch, yaw, latitude, longitude, altitude)
            if out:
                rospy.loginfo("Took-off")
            else:
                rospy.loginfo("Failed taking-off")
        except rospy.ServiceException, e:
            rospy.loginfo("Service call failed")

        # Keep sending take-off messages until received by FCU
        while not out:
            r.sleep()
            out = takeoff(min_pitch, yaw, latitude, longitude, altitude)
            if out:
                rospy.loginfo("Took-off")
            else:
                rospy.loginfo("Failed taking-off")

        while self.gps.altitude< current_altitude+altitude-0.1:
            r.sleep()
            differ = self.gps.altitude - current_altitude
            rospy.loginfo("Waiting to take off, current height %s", differ)

        # Position
        print(self.current_pose.pose.position.x)
        self.goal_pose.header.frame_id = "8"
        self.goal_pose.header.seq = 1
        self.goal_pose.pose.position.x = self.current_pose.pose.position.x + 5
        self.goal_pose.pose.position.y = self.current_pose.pose.position.y + 5
        self.goal_pose.pose.position.z = 3 
        local_position_pub.publish(self.goal_pose)
        rospy.loginfo(self.goal_pose)

        time.sleep(2)
        # Landing
        print "\nLanding"
        rospy.wait_for_service('/mavros/cmd/land')
        try:
            landing_cl = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL)
            response = landing_cl(altitude=alt_set, latitude=0, longitude=0, min_pitch=0, yaw=0)
            rospy.loginfo(response)
        except rospy.ServiceException as e:
            print("Landing failed: %s" %e)
            rospy.signal_shutdown(True)
            rospy.loginfo("\n Pilot Takeover!")
if __name__ == '__main__':
    my_operation = Operation()
    my_operation.start_operation()

MAVROS version and platform

Mavros: ?0.18.4? Version of Mavros was built into an image for the Navio2 Flight Controller. I believe this is the version of Mavros we are using: https://docs.emlid.com/navio2/common/ardupilot/configuring-raspberry-pi/. ROS: Kinetic Raspbian: GNU/Linux 9

Autopilot type and version

[ X ] ArduPilot [ ] PX4

Version: 3.5

Node logs

rosrun mavros mavros_node _fcu_url:=udp://:14650@ _gcs_url:=udp://:14551@192.168.1.90:14550
[ INFO] [1548171941.196073151]: FCU URL: udp://:14650@
[ INFO] [1548171941.207366747]: udp0: Bind address: 0.0.0.0:14650
[ INFO] [1548171941.208232051]: GCS URL: udp://:14551@192.168.1.90:14550
[ INFO] [1548171941.208940691]: udp1: Bind address: 0.0.0.0:14551
[ INFO] [1548171941.209511987]: udp1: Remote address: 192.168.1.90:14550
[ INFO] [1548171941.380572463]: Plugin 3dr_radio loaded
[ INFO] [1548171941.389768786]: Plugin 3dr_radio initialized
[ INFO] [1548171941.390560185]: Plugin actuator_control loaded
[ INFO] [1548171941.417157026]: Plugin actuator_control initialized
[ INFO] [1548171941.418110559]: Plugin altitude loaded
[ INFO] [1548171941.426639127]: Plugin altitude initialized
[ INFO] [1548171941.427587192]: Plugin command loaded
[ INFO] [1548171941.464382795]: Plugin command initialized
[ INFO] [1548171941.465236745]: Plugin ftp loaded
[ INFO] [1548171941.498379360]: udp0: Remote address: 127.0.0.1:48247
[ INFO] [1548171941.522233466]: Plugin ftp initialized
[ INFO] [1548171941.523215749]: Plugin global_position loaded
[ INFO] [1548171941.635482280]: Plugin global_position initialized
[ INFO] [1548171941.636533677]: Plugin hil loaded
[ INFO] [1548171941.755061662]: Plugin hil initialized
[ INFO] [1548171941.756035038]: Plugin home_position loaded
[ INFO] [1548171941.788141152]: Plugin home_position initialized
[ INFO] [1548171941.789200049]: Plugin imu loaded
[ INFO] [1548171941.840590415]: Plugin imu initialized
[ INFO] [1548171941.841528115]: Plugin local_position loaded
[ INFO] [1548171941.873298034]: Plugin local_position initialized
[ INFO] [1548171941.874234224]: Plugin manual_control loaded
[ INFO] [1548171941.901421997]: Plugin manual_control initialized
[ INFO] [1548171941.902161522]: Plugin param loaded
[ INFO] [1548171941.922325609]: Plugin param initialized
[ INFO] [1548171941.923155081]: Plugin rc_io loaded
[ INFO] [1548171941.954899636]: Plugin rc_io initialized
[ INFO] [1548171941.955912648]: Plugin safety_area loaded
[ INFO] [1548171941.991687739]: Plugin safety_area initialized
[ INFO] [1548171941.992652730]: Plugin setpoint_accel loaded
[ INFO] [1548171942.019251343]: Plugin setpoint_accel initialized
[ INFO] [1548171942.020367583]: Plugin setpoint_attitude loaded
[ INFO] [1548171942.089494976]: Plugin setpoint_attitude initialized
[ INFO] [1548171942.090385020]: Plugin setpoint_position loaded
[ INFO] [1548171942.200324127]: Plugin setpoint_position initialized
[ INFO] [1548171942.201273858]: Plugin setpoint_raw loaded
[ INFO] [1548171942.281097821]: Plugin setpoint_raw initialized
[ INFO] [1548171942.282215935]: Plugin setpoint_velocity loaded
[ INFO] [1548171942.333599532]: Plugin setpoint_velocity initialized
[ INFO] [1548171942.334883791]: Plugin sys_status loaded
[ INFO] [1548171942.399843149]: Plugin sys_status initialized
[ INFO] [1548171942.400893140]: Plugin sys_time loaded
[ INFO] [1548171942.445807159]: TM: Timesync mode: MAVLINK
[ INFO] [1548171942.455653059]: Plugin sys_time initialized
[ INFO] [1548171942.456755914]: Plugin vfr_hud loaded
[ INFO] [1548171942.462444925]: Plugin vfr_hud initialized
[ INFO] [1548171942.463514759]: Plugin waypoint loaded
[ INFO] [1548171942.495122024]: Plugin waypoint initialized
[ INFO] [1548171942.496142223]: Plugin wind_estimation loaded
[ INFO] [1548171942.501561861]: Plugin wind_estimation initialized
[ INFO] [1548171942.502018732]: Built-in SIMD instructions: None
[ INFO] [1548171942.502790079]: Built-in MAVLink package version: 2018.8.8
[ INFO] [1548171942.503215388]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1548171942.503826164]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1548171943.498421782]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega / ArduCopter
[ INFO] [1548171944.518478698]: VER: 1.1: Capabilities         0x0000000000001bcf
[ INFO] [1548171944.518661092]: VER: 1.1: Flight software:     030505ff (88a1ecdd)
[ INFO] [1548171944.518815883]: VER: 1.1: Middleware software: 00000000 (        )
[ INFO] [1548171944.519121088]: VER: 1.1: OS software:         00000000 (        )
[ INFO] [1548171944.519234941]: VER: 1.1: Board hardware:      00000000
[ INFO] [1548171944.519333847]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1548171944.519432231]: VER: 1.1: UID:                 0000000000000000
[ WARN] [1548171944.519705770]: CMD: Unexpected command 520, result 0
[ INFO] [1548171953.499576030]: HP: requesting home position
[ INFO] [1548171953.518419457]: FCU: APM:Copter V3.5.5 (88a1ecdd)
[ INFO] [1548171953.519103044]: FCU: Frame: QUAD
[ INFO] [1548171954.278975265]: PR: parameters list received
[ INFO] [1548171958.538548998]: WP: item #0  F:0 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 0.000000 y: 0.000000 z: 0.000000
[ INFO] [1548171958.539139201]: WP: mission received
[ INFO] [1548171963.499519872]: HP: requesting home position

Diagnostics

header:
  seq: 32
  stamp:
    secs: 1548171961
    nsecs:   5035463
  frame_id: ''
status:
  -
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://:14650@"
    values:
      -
        key: "Received packets:"
        value: "792"
      -
        key: "Dropped packets:"
        value: "0"
      -
        key: "Buffer overruns:"
        value: "0"
      -
        key: "Parse errors:"
        value: "0"
      -
        key: "Rx sequence number:"
        value: "104"
      -
        key: "Tx sequence number:"
        value: "6"
      -
        key: "Rx total bytes:"
        value: "25850"
      -
        key: "Tx total bytes:"
        value: "144"
      -
        key: "Rx speed:"
        value: "83.000000"
      -
        key: "Tx speed:"
        value: "0.000000"
  -
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "udp://:14650@"
    values:
      -
        key: "Satellites visible"
        value: "0"
      -
        key: "Fix type"
        value: "0"
      -
        key: "EPH (m)"
        value: "Unknown"
      -
        key: "EPV (m)"
        value: "Unknown"
  -
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://:14650@"
    values:
      -
        key: "Heartbeats since startup"
        value: "18"
      -
        key: "Frequency (Hz)"
        value: "1.000005"
      -
        key: "Vehicle type"
        value: "Quadrotor"
      -
        key: "Autopilot type"
        value: "ArduPilotMega / ArduCopter"
      -
        key: "Mode"
        value: "STABILIZE"
      -
        key: "System status"
        value: "Critical"
  -
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "udp://:14650@"
    values:
      -
        key: "Sensor present"
        value: "0x00000000"
      -
        key: "Sensor enabled"
        value: "0x00000000"
      -
        key: "Sensor helth"
        value: "0x00000000"
      -
        key: "CPU Load (%)"
        value: "0.0"
      -
        key: "Drop rate (%)"
        value: "0.0"
      -
        key: "Errors comm"
        value: "0"
      -
        key: "Errors count #1"
        value: "0"
      -
        key: "Errors count #2"
        value: "0"
      -
        key: "Errors count #3"
        value: "0"
      -
        key: "Errors count #4"
        value: "0"
  -
    level: 2
    name: "mavros: Battery"
    message: "No data"
    hardware_id: "udp://:14650@"
    values:
      -
        key: "Voltage"
        value: "-1.00"
      -
        key: "Current"
        value: "0.0"
      -
        key: "Remaining"
        value: "0.0"

Check ID


WARNING: mavros/target_system_id not set. Used default value: 1
WARNING: mavros/target_component_id not set. Used default value: 1
NOTE: Target parameters may be unset, but that may be result of incorrect --mavros-ns option.Double check results!
OK. I got messages from 1:1.

---
Received 16 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 77```

SchaeferB1 commented 5 years ago

@vooon @TSC21 I'd really appreciate your expertise in this matter, if possible. Thank you!

TSC21 commented 5 years ago

@YCH188 stop spamming the issues list please. Create your own issue of you want to ask questions, but only after you empty the other options, which is asking your questions on the appropriate forums.

sergioma295 commented 5 years ago

I am having a big problem. I have created a new issue. Can somebody help me please? #1248

erikasnow commented 4 years ago

@SchaeferB1 I'm trying to do something very similar to what you described. However, I can't seem to get the setpoint_position/local message through to the pixhawk. The topic updates correctly, but I'm not seeing the corresponding message in Mavlink Inspector. Did you ever run into any problems like this?

ericjohnson97 commented 4 years ago

There are a couple things that need to happen when using setpoint position local. some of the things that I struggled with when learning mavros were:

for ardupilot the drone must first be in the air to use setpoint position local
on an older version of mavros the origin for the setpoint position local plugin origin was set to the center of the earth instead of the initial GPS pose. this should have been fixed in later releases after june 2019

Here are some good tutorials using mavros for local navigation with some example code. https://github.com/Intelligent-Quads/iq_tutorials

these are also some good reads https://github.com/mavlink/mavros/issues/906

https://github.com/mavlink/mavros/issues/1213

mavlink / mavros