Robotics StackExchange | Archived questions

simulate python scrpits on gazebo

I want to test my scripts on gazebo but I don't know how can I edit my scripts for . Actually I have a script and it works in real but I can't see action on gazebo. So I need help, What I have to do before rosrun my scrpits (drawline.py)

drawline.py:

#!/usr/bin/env python


## TODO:: fix: during rospy.sleep(2.0) velocity still remians 2.0, DAC10 does not work, Because of sleep Drone goes 4meter more
##
##

import rospy
from std_msgs.msg import String, Float64
from mavros_msgs.msg import State, HomePosition
from sensor_msgs.msg import NavSatFix
from mavros_msgs.srv import *
from geometry_msgs.msg import Twist
from decimal import *
from math import radians, cos, sin, asin, sqrt


# Message publisher
velocity_pub = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel_unstamped', Twist, queue_size=10)
msg = Twist()


# Current Position
latitude = 0.0
longitude = 0.0
altitude = 0.0


# Position before Move function execute
previous_latitude = 0.0
previous_longitude = 0.0
previous_altitude = 0.0


# Drone State Status
stateConnected = False
stateArmed = False
stateGuided = False




# ########################################################################
# ------------------------------- SERVICES -------------------------------
# ########################################################################

def setArm():
  rospy.wait_for_service('/mavros/cmd/arming')
  try:
    armService = rospy.ServiceProxy(
        '/mavros/cmd/arming', mavros_msgs.srv.CommandBool)
    armService(True)
  except rospy.ServiceException, e:
    print "Service arm call failed: %s" % e



def setTakeoffMode(alt):
  rospy.wait_for_service('/mavros/cmd/takeoff')
  try:
    takeoffService = rospy.ServiceProxy(
        '/mavros/cmd/takeoff', mavros_msgs.srv.CommandTOL)
    takeoffService(altitude=int(alt))
  except rospy.ServiceException, e:
    print "Service takeoff call failed: %s" % e



def setLandMode():
  rospy.wait_for_service('/mavros/cmd/land')
  try:
    takeoffService = rospy.ServiceProxy(
        '/mavros/cmd/land', mavros_msgs.srv.CommandTOL)
    takeoffService(altitude=0)
  except rospy.ServiceException, e:
    print "Service takeoff call failed: %s" % e



def setGuidedMode():
    rospy.wait_for_service('/mavros/set_mode')
    try:
        flightModeService = rospy.ServiceProxy('/mavros/set_mode', mavros_msgs.srv.SetMode)
        #http://wiki.ros.org/mavros/CustomModes for custom modes
        isModeChanged = flightModeService(custom_mode='GUIDED') #return true or false
    except rospy.ServiceException, e:
        print "service set_mode GUIDED call failed: %s. GUIDED Mode could not be set. Check that GPS is enabled"%e





# ###############################################################################
# ------------------------------- END OF SERVICES -------------------------------
# ###############################################################################


# ###############################################################################
# ------------------------------- CALLBACK FUNCTIONS ----------------------------
# ###############################################################################

def globalPositionCallback(globalPositionCallback):
    global latitude
    global longitude
    latitude = globalPositionCallback.latitude
    longitude = globalPositionCallback.longitude

def globalAltitudeCallback(globalAltitudeCB):
    global altitude
    altitude = globalAltitudeCB.data

def globalStateCallback(globalStateCB):
    global stateConnected
    global stateArmed
    global stateGuided
    stateConnected = globalStateCB.connected
    stateArmed = globalStateCB.armed
    stateGuided = globalStateCB.guided

"""
def globalHomePositionCallback(globalHomePositionCallback):
    global home_altitude
    global home_longitude
    global home_latitude
    home_altitude = globalHomePositionCallback.geo.altitude
    home_latitude = globalHomePositionCallback.geo.latitude
    home_longitude = globalHomePositionCallback.geo.longitude"""


# ###############################################################################
# ------------------------------- END OF CALLBACK -------------------------------
# ###############################################################################


# Function to calculate the distance of 2 points in earth. 
# Uses current position from gps and data before movement.
# Return value will always positive no matter what direction drone goes.
# Using haversine formula to calculate distance
def haversine():
    global previous_longitude
    global previous_latitude
    global longitude
    global latitude



    lon1, lat1, lon2, lat2 = previous_longitude, previous_latitude, longitude, latitude

    # lat, long in radians
    lon1, lat1, lon2, lat2 = map(radians, [lon1, lat1, lon2, lat2])

    # difference lat, long
    dlon = abs(lon2 - lon1) 
    dlat = abs(lat2 - lat1) 
    ## haversine formula 
    a = sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
    c = 2 * asin(sqrt(a)) 
    r = 6371 # Radius of earth in kilometers
    return Decimal(c * r * 1000)




# Distance must be meter
# Pass argument speed as float if possible
def moveX(distance, speed):
    # for print statements, debugging, delete later
    global latitude
    global longitude
    global previous_latitude
    global previous_longitude
    # end of debug

    msg.linear.x = speed

    while not rospy.is_shutdown():
        # compute the distance with haversine formula
        # home_latitude = starting latitude before movement
        # home longitude = starting longitude before movement
        # latitude = current latitude during move
        # longitude = current longitude during move
        haversine_distance = haversine()
        # break loop if desired distance covered 
        if haversine_distance > distance:
            break

        velocity_pub.publish(msg)

        # This line might cause error because function have no access to rate object in main. 
        # Should i move into global space or pass as argument
        rate.sleep()

        # TODO:: lower speed while getting closer to the final position
        # TODO:: check if rospy.sleep() or rate.sleep() is better
        # TODO:: is creating rate object necessary?

    print("Latitude: {:.7f} ,Longitude: {:.7f}\nDistance Covered: {:.7f}".format(latitude, longitude, haversine_distance))

    # Tell drone to stop its movement
    # Not sure if this works. Need to test on gazebo
    msg.linear.x = 0.
    msg.linear.y = 0.
    msg.linear.z = 0.
    for i in range(100):
        velocity_pub.publish(msg)

    # DEBUG
    print("Speed X: {} ,Speed Y: {} ,Speed Z: {} ".format(msg.linear.x, msg.linear.y, msg.linear.z))

    rospy.sleep(1.)

    # after reaching desired position, set home_latitude, home_longitude to current position value
    previous_latitude = latitude
    previous_longitude = longitude




# Distance must be meter
# Pass argument speed as float if possible
def moveY(distance, speed):
    # for print statements, debugging, delete later
    global latitude
    global longitude
    global previous_latitude
    global previous_longitude
    # end of debug

    msg.linear.y = speed

    while not rospy.is_shutdown():
        # compute the distance with haversine formula
        # home_latitude = starting latitude before movement
        # home longitude = starting longitude before movement
        # latitude = current latitude during move
        # longitude = current longitude during move
        haversine_distance = haversine()
        # break loop if desired distance covered 
        if haversine_distance > distance:
            break

        velocity_pub.publish(msg)

        # This line might cause error because function have no access to rate object in main. 
        # Should i move into global space or pass as argument
        rate.sleep()

        # TODO:: lower speed while getting closer to the final position
        # TODO:: check if rospy.sleep() or rate.sleep() is better
        # TODO:: is creating rate object necessary?

    print("Latitude: {:.7f} ,Longitude: {:.7f}\nDistance Covered: {:.7f}".format(latitude, longitude, haversine_distance))

    # Tell drone to stop its movement
    # Not sure if this works. Need to test on gazebo
    msg.linear.x = 0.
    msg.linear.y = 0.
    msg.linear.z = 0.
    for i in range(100):
        velocity_pub.publish(msg)

    # DEBUG
    print("Speed X: {} ,Speed Y: {} ,Speed Z: {} ".format(msg.linear.x, msg.linear.y, msg.linear.z))

    rospy.sleep(1.)

    # after reaching desired position, set home_latitude, home_longitude to current position value
    previous_latitude = latitude
    previous_longitude = longitude



"""
def moveWithImage(data):
    # TODO:: Implement move using data
    # Data (string x,y,Radius)

    XY = data.data
    XY = XY.split(",")

    x = float(XY[0])
    y = float(XY[1])
    radius = float(XY[2])
    print("X:{:.3f}   Y:{:.3f}   Radius:{:.3f}".format(x,y,radius))

    # x or y not equal to 0 ?
    if not x or not y:
        move('-x', 40, 1)
    else:
        msg.linear.x = x
        msg.linear.y = y
        msg.linear.z = -0.5 # -1 go down  ?
    # z = ?
"""


def testDrawSquare4M():
    moveX(4, 2)
    rospy.sleep(.5)
    moveY(4, 2)
    rospy.sleep(.5)
    moveX(4, -2)
    rospy.sleep(.5)
    moveY(4, -2)
    rospy.sleep(1.)






if __name__ == '__main__':

    rospy.init_node('drone', anonymous=True)
    rate = rospy.Rate(10)
    rospy.Subscriber("/mavros/global_position/raw/fix", NavSatFix, globalPositionCallback)
    rospy.Subscriber("mavros/global_position/rel_alt", Float64, globalAltitudeCallback)
    rospy.Subscriber("mavros/state", State, globalStateCallback)
    """
    rospy.Subscriber("mavros/home_position/home", HomePosition, globalHomePositionCallback)"""


    while not stateConnected:
        rospy.sleep(1.)
    # You can define publisher as global variable without initializing it.
    # You can later initialize publisher with node handler within main() after ros::init().
    # In this way you can use publisher outside main within any function.


    # Services executes on another thread. Sleeping rospy is a must!

    setGuidedMode()
    while not stateGuided:
        rospy.sleep(.1)

    print("Drone Guided:{}".format(stateGuided))

    # Check if drone is armed
    setArm()
    while not stateArmed:
        rospy.sleep(.1)
        setArm()

    rospy.sleep(3.)

    print("Drone Armed:{}".format(stateArmed))
    # UP until desired altitude
    setTakeoffMode(6)
    while altitude < 5.:
        rospy.sleep(.5)


    previous_longitude = longitude
    previous_latitude = latitude


    moveX(3, 2)

    rospy.sleep(5.)

    # END OF MISSION
    setLandMode()

this is my command line for launch gazebo:

abdulsamet@AT:~/ros/PX4-Autopilot$ roslaunch px4 posix_sitl.launch

Asked by abdupa on 2020-12-08 12:38:19 UTC

Comments

Answers