diff --git a/launch/innosim_relay.launch b/launch/innosim_relay.launch deleted file mode 100644 index 1a1fe6a..0000000 --- a/launch/innosim_relay.launch +++ /dev/null @@ -1,23 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - diff --git a/scripts/gimbal_control_node.py b/scripts/gimbal_control_node.py deleted file mode 100755 index d5841d6..0000000 --- a/scripts/gimbal_control_node.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python -import roslib -import rospy -import math -import tf -import geometry_msgs.msg -from sensor_msgs.msg import CameraInfo - -# https://github.com/carla-simulator/scenario_runner/blob/master/srunner/challenge/autoagents/ros_agent.py -def build_camera_info(attributes): - """ - Private function to compute camera info - camera info doesn't change over time - """ - camera_info = CameraInfo() - # store info without header - camera_info.header.frame_id = "velodyne" - camera_info.width = int(attributes['width']) - camera_info.height = int(attributes['height']) - camera_info.distortion_model = 'plumb_bob' - cx = camera_info.width / 2.0 - cy = camera_info.height / 2.0 - fx = camera_info.width / ( - 2.0 * math.tan(float(attributes['fov']) * math.pi / 360.0)) - fy = fx - camera_info.K = [fx, 0, cx, 0, fy, cy, 0, 0, 1] - camera_info.D = [0, 0, 0, 0, 0] - camera_info.R = [1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0] - camera_info.P = [fx, 0, cx, 0, 0, fy, cy, 0, 0, 0, 1.0, 0] - return camera_info - -if __name__ == '__main__': - rospy.init_node('gimbal_control') - - listener = tf.TransformListener() - - gimbal_angle_pub = rospy.Publisher('/sim/gimbal_angle', geometry_msgs.msg.Vector3Stamped, queue_size=1) - camera_info_pub = rospy.Publisher('/sim/camera_info', CameraInfo, queue_size=1) - - rate = rospy.Rate(100.0) - - br = tf.TransformBroadcaster() - attributes = dict() - attributes['width'] = 1920 - attributes['height'] = 1080 - attributes['fov'] = 26.9914 - camera_info = build_camera_info(attributes) - - while not rospy.is_shutdown(): - try: - # listener.waitForTransform('/map', '/base_link', rospy.Time.now(), rospy.Duration(2.0)) - now = rospy.Time.now() - (trans,rot) = listener.lookupTransform('/map', '/base_link', rospy.Time(0)) - - (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(rot) - - msg = geometry_msgs.msg.Vector3Stamped() - msg.header.stamp = now - - msg.vector.x = 0.0 # math.degrees(roll) - msg.vector.y = 90.0 # math.degrees(pitch) - msg.vector.z = 90 - math.degrees(yaw) - - gimbal_angle_pub.publish(msg) - - br.sendTransform(trans, - tf.transformations.quaternion_from_euler(0, math.radians(30), yaw), - now, - "gimbal", - "map") - - except (tf.Exception): - continue - - rate.sleep() - - camera_info.header.stamp = rospy.Time.now() - camera_info_pub.publish(camera_info) diff --git a/scripts/innosim_relay_node.py b/scripts/innosim_relay_node.py deleted file mode 100755 index 97b5c3b..0000000 --- a/scripts/innosim_relay_node.py +++ /dev/null @@ -1,125 +0,0 @@ -#!/usr/bin/env python - -import rospy -from geometry_msgs.msg import QuaternionStamped -from nav_msgs.msg import Odometry -from mavros_msgs.msg import RCOut -from sensor_msgs.msg import Joy -from sensor_msgs.msg import NavSatFix - -attitude_pub = rospy.Publisher('/sim/attitude', QuaternionStamped, queue_size = 1) -joy_pub = rospy.Publisher('/sim/actuators', Joy, queue_size = 1) - -gps_pub = rospy.Publisher('/sim/gps_position', NavSatFix, queue_size = 1) - - -# Here is some magic needed to give global altitude to simulator. -# GLOBAL_POSITION_INT (/mavros/global_position/global) has altitude relative to starting point. -# GPS_RAW_INT (/mavros/global_position/raw/fix) has global altitude (we could tune it with `export PX4_HOME_ALT`), but publishing rate is low. -# So, we apply altitude correction in GLOBAL_POSITION_INT and send to sim. -global raw_alt, alt_init, alt_corr, has_raw_alt -raw_alt = 0.0 -alt_init = False -alt_corr = 0.0 -has_raw_alt = False - -def global_gps_callback(data): - global raw_alt, alt_init, alt_corr, has_raw_alt - - if alt_init: - data.altitude += alt_corr - gps_pub.publish(data) - else: - if has_raw_alt: - alt_corr = raw_alt - data.altitude - alt_init = True - -def raw_gps_callback(data): - global raw_alt, has_raw_alt - - raw_alt = data.altitude - has_raw_alt = True - -def odom_callback(data): - q = QuaternionStamped() - q.header = data.header - q.quaternion = data.pose.pose.orientation - attitude_pub.publish(q) - -def rc_callback(data): - # PX4 SITL VTOL channels: - # 1 FR, ccw (900 - 2000) - # 2 RL, ccw - # 3 FL, cw - # 4 RR, cw (1000 - 2000) - # 5 pusher (1000 - 2000) - # 6 aileron (1000 - 2000) - # 7 aileron - # 8 elevator - FR_pwm = data.channels[0] - RL_pwm = data.channels[1] - FL_pwm = data.channels[2] - RR_pwm = data.channels[3] - pusher_pwm = data.channels[4] - aileron1_pwm = data.channels[5] - aileron2_pwm = data.channels[6] - elevator_pwm = data.channels[7] - - - joy = Joy() - # Sim: - # FR, cw, rate, rpm (Front right motor speed) - # RL, cw, rate, rpm (Rear left motor speed) - # FL, ccw, rate, rpm (Front left motor speed) - # RR, ccw, rate, rpm (Rear right motor speed) - # aileron left, cw, deg - # aileron right, cw, deg - # elevator, cw, deg - # rudder, cw, deg - # thrust, pusher, rate, rpm - - FR_res = 15500*(FR_pwm - 900.0)/1100.0 - RL_res = 15500*(RL_pwm - 900.0)/1100.0 - FL_res = 15500*(FL_pwm - 900.0)/1100.0 - RR_res = 15500*(RR_pwm - 900.0)/1100.0 - aileron1_res = 70*(aileron1_pwm - 1500.0)/500.0 - aileron2_res = 70*(aileron2_pwm - 1500.0)/500.0 - elevator_res = 70*(elevator_pwm - 1500.0)/500.0 - rudder_res = 0.0 - thrust_res = 15500*(pusher_pwm - 1000.0)/1000.0 - - joy.axes.append( round( FR_res, 1) ) - joy.axes.append( round( RL_res, 1) ) - joy.axes.append( round( FL_res, 1) ) - joy.axes.append( round( RR_res, 1) ) - joy.axes.append( round( aileron1_res, 1) ) - joy.axes.append( round( aileron2_res, 1 ) ) - joy.axes.append( round( elevator_res, 1) ) - joy.axes.append( round( rudder_res, 1) ) - joy.axes.append( round( thrust_res, 1) ) - - for i in range(4): - if joy.axes[i] < 0: - joy.axes[i] = 0 - - if joy.axes[8] < 0: - joy.axes[8] = 0 - - joy_pub.publish(joy) - - -def innosim_relay(): - rospy.init_node('innosim_relay', anonymous=True) - rospy.Subscriber('/mavros/global_position/local', Odometry, odom_callback) - rospy.Subscriber('/mavros/rc/out', RCOut, rc_callback) - rospy.Subscriber('/mavros/global_position/global', NavSatFix, global_gps_callback) - rospy.Subscriber('/mavros/global_position/raw/fix', NavSatFix, raw_gps_callback) - rospy.spin() - - -if __name__ == '__main__': - try: - innosim_relay() - except rospy.ROSInterruptException: - pass - diff --git a/scripts/innosim_vtol_bridge.py b/scripts/innosim_vtol_bridge.py deleted file mode 100755 index ded6663..0000000 --- a/scripts/innosim_vtol_bridge.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python3 - -import rospy -from sensor_msgs.msg import NavSatFix -from geometry_msgs.msg import QuaternionStamped -from sensor_msgs.msg import Joy -from pyquaternion import Quaternion - -FRD_FLU = Quaternion(w=0, x=1, y=0, z=0) -NED_TO_ENU = Quaternion(w=0, x=0.70711, y=0.70711, z=0) - -class InnoSimBridge: - def __init__(self): - self.attitude_pub = rospy.Publisher('/sim/attitude', QuaternionStamped, queue_size = 1) - self.joy_pub = rospy.Publisher('/sim/actuators', Joy, queue_size = 1) - self.gps_pub = rospy.Publisher('/sim/gps_position', NavSatFix, queue_size = 1) - - rospy.Subscriber('/uav/gps_point', NavSatFix, self.gps_callback) - rospy.Subscriber('/uav/attitude', QuaternionStamped, self.attitude_callback) - rospy.Subscriber('/uav/actuators', Joy, self.actuators_callback) - - self.gps_msg = NavSatFix() - self.joy = Joy() - self.joy.axes = [0] * 9 - - def gps_callback(self, in_msg): - self.gps_msg.header.stamp = in_msg.header.stamp - self.gps_msg.latitude = in_msg.latitude * 1e-08 - self.gps_msg.longitude = in_msg.longitude * 1e-08 - self.gps_msg.altitude = in_msg.altitude * 1e-03 + 6.5 - self.gps_pub.publish(self.gps_msg) - - def attitude_callback(self, msg): - # We receive altitude in PX4 notation, but we need to publish in ROS notation - # So, at first we convert NED to baselink - # After that we convert baselink to ENU - q = Quaternion(w=msg.quaternion.w, - x=msg.quaternion.x, - y=msg.quaternion.y, - z=msg.quaternion.z) - - q = NED_TO_ENU * q * FRD_FLU - - msg.quaternion.w = q.w - msg.quaternion.x = q.x - msg.quaternion.y = q.y - msg.quaternion.z = q.z - - self.attitude_pub.publish(msg) - - def actuators_callback(self, actuators): - try: - if len(actuators.axes) < 4: - rospy.logerr_throttle(5, "InnosimUavcanBridge: wrong actuators msg len(axes) < 4") - return - elif len(actuators.axes) != 8: - rospy.logwarn_throttle(5, "InnosimUavcanBridge: wrong airframe. Let it be MC.") - except: - rospy.logerr_throttle(5, "InnosimUavcanBridge: wrong actuators msg. There is no axes") - return - - MIXER_MIN_MAX = [ - (0, 1), # 0. FR is normalized into [0, +1], where 0 means turn off - (0, 1), # 1. RL is normalized into [0, +1], where 0 means turn off - (0, 1), # 2. FL motor is normalized into [0, +1], where 0 means turn off - (0, 1), # 3. RR motor is normalized into [0, +1], where 0 means turn off - (0, 1), # 4. Aileron is normalized into [0, +1], where 0.5 is a middle position - (-1, 1),# 5. Elevator is normalized into [-1, +1], where 0 is a middle position - (-1, 1),# 6. Rudder is normalized into [-1, +1], where 0 is a middle position - (0, 1), # 7. Pusher is normalized into [0, +1], where 0 means turn off - ] - - # 1. Clamp input data and convert it to the px4 control group format (from -1 to +1) - mixer = [0] * min(len(MIXER_MIN_MAX), len(actuators.axes)) - for idx in range(len(mixer)): - mixer[idx] = max(MIXER_MIN_MAX[idx][0], min(actuators.axes[idx], MIXER_MIN_MAX[idx][1])) - if len(mixer) == 8: - mixer[4] = (mixer[4] - 0.5) * 2 - - # 2. Convert them to the sim format - SIM_MAX_VALUES = [ - 15500, # FR, cw, rate, rpm (Front right motor speed) - 15500, # RL, cw, rate, rpm (Rear left motor speed) - 15500, # FL, ccw, rate, rpm (Front left motor speed) - 15500, # RR, ccw, rate, rpm (Rear right motor speed) - 70, # aileron left, cw, deg - 70, # aileron right, cw, deg - 70, # elevator, cw, deg - 70, # rudder, cw, deg - 15500 # thrust, pusher, rate, rpm - ] - - for idx in range(min(4, len(mixer))): - self.joy.axes[idx] = mixer[idx] * SIM_MAX_VALUES[idx] - if len(mixer) == 8: - self.joy.axes[4] = mixer[4] * SIM_MAX_VALUES[4] - self.joy.axes[5] = -mixer[4] * SIM_MAX_VALUES[5] - self.joy.axes[6] = mixer[5] * SIM_MAX_VALUES[6] - self.joy.axes[7] = mixer[6] * SIM_MAX_VALUES[7] - self.joy.axes[8] = mixer[7] * SIM_MAX_VALUES[8] - - self.joy_pub.publish(self.joy) - - -if __name__ == '__main__': - try: - rospy.init_node('innosim_relay', anonymous=True) - inno_sim_bridge = InnoSimBridge() - rospy.spin() - except rospy.ROSInterruptException: - pass - diff --git a/scripts/run_all.sh b/scripts/run_all.sh deleted file mode 100755 index df4e1d6..0000000 --- a/scripts/run_all.sh +++ /dev/null @@ -1,58 +0,0 @@ -#!/bin/bash - -FIRMWARE_PATH="~/Work/vtol/Firmware" -INNOSIM_PATH="~/Work/InnoSim/Linux" -QGC_PATH="~/Work" - -START_LAT="55.7544426" -START_LON="48.742684" -START_ALT="-6.5" - - -tmux start-server - -sleep 1 - -tmux new -s innosim -d -tmux rename-window -t innosim innosim - - -tmux split-window -v -t innosim - -tmux select-pane -t innosim:0.0 -tmux split-window -h -t innosim -tmux split-window -h -t innosim - -tmux select-pane -t innosim:0.3 -tmux split-window -h -t innosim -tmux split-window -h -t innosim - - -tmux select-pane -t innosim:0.2 -tmux send-keys "cd $FIRMWARE_PATH -export PX4_HOME_LAT=$START_LAT -export PX4_HOME_LON=$START_LON -export PX4_HOME_ALT=$START_ALT -make px4_sitl gazebo_standard_vtol" C-m - -tmux select-pane -t innosim:0.3 -tmux send-keys "roscd inno_sim_interface/cfg -$INNOSIM_PATH/InnoSimulator.x86_64 --config config.yaml" C-m - -tmux select-pane -t innosim:0.4 -tmux send-keys "$QGC_PATH/QGroundControl.AppImage" C-m - -sleep 1 - -tmux select-pane -t innosim:0.0 -tmux send-keys 'roslaunch inno_sim_interface innosim_relay.launch' C-m - -sleep 1 - -tmux select-pane -t innosim:0.1 -tmux send-keys "roslaunch rosbridge_server rosbridge_websocket.launch" C-m - - -tmux select-pane -t innosim:0.5 - -tmux attach -t innosim \ No newline at end of file diff --git a/scripts/sim2rviz.py b/scripts/sim2rviz.py deleted file mode 100755 index 0cfa9e6..0000000 --- a/scripts/sim2rviz.py +++ /dev/null @@ -1,72 +0,0 @@ -import rospy -import pyproj -import utm -from geometry_msgs.msg import PoseStamped, QuaternionStamped, Quaternion, Pose, Twist, Vector3 -from sensor_msgs.msg import NavSatFix -from nav_msgs.msg import Odometry - -# Initialize node -rospy.init_node('gps_attitude_to_pose') - -# Set up publishers and subscribers -pose_pub = rospy.Publisher('/drone_pose', PoseStamped, queue_size=1) -odom_pub = rospy.Publisher('/drone_odom', Odometry, queue_size=1) -attitude = Quaternion() -position = NavSatFix() - -# Variables to hold the origin of the local frame (first received GPS data) -origin_lat = None -origin_lon = None -origin_east = None -origin_north = None -origin_zone = None -origin_letter = None - -def attitude_callback(msg): - global attitude - attitude = msg.quaternion - -def gps_callback(msg): - global position, origin_lat, origin_lon, origin_east, origin_north, origin_zone, origin_letter - position = msg - - if origin_lat is None and origin_lon is None: - # Store the first received GPS data as the origin of our local frame - origin_lat = position.latitude - origin_lon = position.longitude - origin_east, origin_north, origin_zone, origin_letter = utm.from_latlon(origin_lat, origin_lon) - - else: - pose = PoseStamped() - pose.header = msg.header - pose.header.frame_id = "world" - - # Convert from LLA to UTM - east, north, _, _ = utm.from_latlon(position.latitude, position.longitude, force_zone_number=origin_zone, force_zone_letter=origin_letter) - - # Subtract the first position in UTM format - pose.pose.position.x = east - origin_east - pose.pose.position.y = north - origin_north - pose.pose.position.z = position.altitude - - # Orientation - pose.pose.orientation = attitude - - # Publishing the PoseStamped message - pose_pub.publish(pose) - - # Creating and publishing the Odometry message - odom_msg = Odometry() - odom_msg.header = pose.header - odom_msg.child_frame_id = "base_link" - odom_msg.pose.pose = pose.pose - odom_msg.twist.twist = Twist(Vector3(0, 0, 0), Vector3(0, 0, 0)) # Assuming velocities are zero - odom_pub.publish(odom_msg) - -attitude_sub = rospy.Subscriber('/sim/attitude', QuaternionStamped, attitude_callback) -gps_sub = rospy.Subscriber('/sim/gps_position', NavSatFix, gps_callback) - -rate = rospy.Rate(10) # 10 Hz - -while not rospy.is_shutdown(): - rate.sleep()