diff --git a/PythonClient/ROS/car_image_raw.py b/PythonClient/ROS/car_image_raw.py new file mode 100644 index 0000000000..fcef0da045 --- /dev/null +++ b/PythonClient/ROS/car_image_raw.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python + +# Example ROS node for publishing AirSim images. + +import rospy + +# ROS Image message +from sensor_msgs.msg import Image + +# AirSim Python API +from AirSimClient import * + +def airpub(): + pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1) + rospy.init_node('image_raw', anonymous=True) + rate = rospy.Rate(10) # 10hz + + # connect to the AirSim simulator + client = CarClient() + client.confirmConnection() + + while not rospy.is_shutdown(): + # get camera images from the car + responses = client.simGetImages([ + ImageRequest(1, AirSimImageType.Scene, False, False)]) #scene vision image in uncompressed RGBA array + + for response in responses: + img_rgba_string = response.image_data_uint8 + + # Populate image message + msg=Image() + msg.header.stamp = rospy.Time.now() + msg.header.frame_id = "frameId" + msg.encoding = "rgba8" + msg.height = 360 # resolution should match values in settings.json + msg.width = 640 + msg.data = img_rgba_string + msg.is_bigendian = 0 + msg.step = msg.width * 4 + + # log time and size of published image + rospy.loginfo(len(response.image_data_uint8)) + # publish image message + pub.publish(msg) + # sleep until next cycle + rate.sleep() + + +if __name__ == '__main__': + try: + airpub() + except rospy.ROSInterruptException: + pass diff --git a/PythonClient/ROS/car_pose.py b/PythonClient/ROS/car_pose.py new file mode 100644 index 0000000000..88e2946d11 --- /dev/null +++ b/PythonClient/ROS/car_pose.py @@ -0,0 +1,57 @@ +#!/usr/bin/env python + +import rospy +import tf +from std_msgs.msg import String +from geometry_msgs.msg import PoseStamped +from AirSimClient import * +import time + + +def airpub(): + pub = rospy.Publisher("airsimPose", PoseStamped, queue_size=1) + rospy.init_node('airpub', anonymous=True) + rate = rospy.Rate(10) # 10hz + + # connect to the AirSim simulator + client = CarClient() + client.confirmConnection() + +# start = time.time() + + + while not rospy.is_shutdown(): + + # get state of the car + car_state = client.getCarState() + pos = car_state.kinematics_true.position + orientation = car_state.kinematics_true.orientation +# milliseconds = (time.time() - start) * 1000 + + + # populate PoseStamped ros message + simPose = PoseStamped() + simPose.pose.position.x = pos.x_val + simPose.pose.position.y = pos.y_val + simPose.pose.position.z = pos.z_val + simPose.pose.orientation.w = orientation.w_val + simPose.pose.orientation.x = orientation.x_val + simPose.pose.orientation.y = orientation.y_val + simPose.pose.orientation.z = orientation.z_val + simPose.header.stamp = rospy.Time.now() + simPose.header.seq = 1 + simPose.header.frame_id = "simFrame" + + # log PoseStamped message + rospy.loginfo(simPose) + #publish PoseStamped message + pub.publish(simPose) + # sleeps until next cycle + rate.sleep() + + +if __name__ == '__main__': + try: + airpub() + except rospy.ROSInterruptException: + pass diff --git a/PythonClient/ROS/drone_image_raw.py b/PythonClient/ROS/drone_image_raw.py new file mode 100644 index 0000000000..9b0d063a25 --- /dev/null +++ b/PythonClient/ROS/drone_image_raw.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python + +# Example ROS node for publishing AirSim images. + +import rospy + +# ROS Image message +from sensor_msgs.msg import Image + +# AirSim Python API +from AirSimClient import * + +def airpub(): + pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1) + rospy.init_node('image_raw', anonymous=True) + rate = rospy.Rate(10) # 10hz + + # connect to the AirSim simulator + client = MultirotorClient() + client.confirmConnection() + + while not rospy.is_shutdown(): + # get camera images from the car + responses = client.simGetImages([ + ImageRequest(1, AirSimImageType.Scene, False, False)]) #scene vision image in uncompressed RGBA array + + for response in responses: + img_rgba_string = response.image_data_uint8 + + # Populate image message + msg=Image() + msg.header.stamp = rospy.Time.now() + msg.header.frame_id = "frameId" + msg.encoding = "rgba8" + msg.height = 360 # resolution should match values in settings.json + msg.width = 640 + msg.data = img_rgba_string + msg.is_bigendian = 0 + msg.step = msg.width * 4 + + # log time and size of published image + rospy.loginfo(len(response.image_data_uint8)) + # publish image message + pub.publish(msg) + # sleep until next cycle + rate.sleep() + + +if __name__ == '__main__': + try: + airpub() + except rospy.ROSInterruptException: + pass diff --git a/docs/ros.md b/docs/ros.md new file mode 100644 index 0000000000..4d91dc4f4f --- /dev/null +++ b/docs/ros.md @@ -0,0 +1,49 @@ +# How to use AirSim with Robot Operating System (ROS) + +AirSim and ROS can be integrated using C++ or Python. Some example ROS nodes are provided demonstrating how to publish +data from AirSim as ROS topics. + +# Python + +## Prerequisites + +These instructions are for Ubuntu 16.04, ROS Kinetic, UE4 4.18 or higher, and latest AirSim release. +You should have these components installed and working before proceding. + +## Setup + + +### Create a new ROS package in your catkin workspace following these instructions. + +Create a new ROS package called airsim or whatever you like. + +[Create ROS package](http://wiki.ros.org/ROS/Tutorials/CreatingPackage) + +If you don't already have a catkin workspace, you should first work through the ROS beginner tutorials. + +### Add AirSim ROS node examples to ROS package + +In the ROS package directory you made, run '''mkdir scripts''' to create a folder for your Python code. +Copy the ros examples from the AirSim/PythonClient directory to your ROS package. Change the code below to match +your AirSim and catkin workspace paths. + +```cp AirSim/PythonClient/ROS/*.py ../catkin_ws/src/airsim/scripts``` + + +### Build ROS AirSim package + +Change directory to your top level catkin workspace folder i.e. ```cd ~/catkin_ws``` and run ```catkin_make``` +This will build the airsim package. Next, run ```source devel/setup.bash``` so ROS can find the new package. +You can add this command to your ~/.bashrc to load your catkin workspace automatically. + +## How to run ROS AirSim nodes + +First make sure UE4 is running an airsim project, the car or drone should be selected, and the simulations is playing. +Examples support car or drone. Make sure to have the correct vehicle for the ros example running. + +The example airsim nodes can be run using ```rosrun airsim example_name.py``` The output of the node +can be viewed in another terminal by running ```rostopic echo /example_name``` You can view a list of the +topics currently published via tab completion after typing ```rostopic echo``` in the terminal. +Rviz is a useful visualization tool that can display the published data. + +# C++ (coming soon)