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