simulate python scrpits on gazebo

asked 2020-12-08 11:38:19 -0500

abdupa gravatar image

updated 2020-12-09 00:56:01 -0500

mgruhler gravatar image

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 ...
(more)
edit retag flag offensive close merge delete