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 ...