Skip to content
Snippets Groups Projects
Commit 6f44b4d3 authored by Andreas Antener's avatar Andreas Antener
Browse files

remove gazebo plugin command fake, fix some lint warnings

parent 4b65e625
No related branches found
No related tags found
No related merge requests found
......@@ -35,7 +35,6 @@
#
# @author Andreas Antener <andreas@uaventure.com>
#
import sys
import rospy
from px4.msg import manual_control_setpoint
......@@ -46,17 +45,12 @@ from std_msgs.msg import Header
#
# Manual input control helper
#
# FIXME: this is not the way to do it! ATM it fakes input to iris/command/attitude because else
# the simulator does not instantiate our controller. Probably this whole class will disappear once
# arming works correctly.
#
class ManualInput:
class ManualInput(object):
def __init__(self):
rospy.init_node('test_node', anonymous=True)
self.pubMcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10)
self.pubOff = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10)
self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10)
self.pub_mcsp = rospy.Publisher('iris/manual_control_setpoint', manual_control_setpoint, queue_size=10)
self.pub_off = rospy.Publisher('iris/offboard_control_mode', offboard_control_mode, queue_size=10)
def arm(self):
rate = rospy.Rate(10) # 10hz
......@@ -81,9 +75,7 @@ class ManualInput:
rospy.loginfo("zeroing")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
# Fake input to iris commander
self.pubAtt.publish(att)
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
......@@ -93,7 +85,7 @@ class ManualInput:
rospy.loginfo("arming")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
......@@ -118,7 +110,7 @@ class ManualInput:
rospy.loginfo("triggering posctl")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
......@@ -147,7 +139,7 @@ class ManualInput:
rospy.loginfo("setting offboard mode")
time = rospy.get_rostime().now()
mode.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubOff.publish(mode)
self.pub_off.publish(mode)
rate.sleep()
count = count + 1
......@@ -169,7 +161,7 @@ class ManualInput:
rospy.loginfo("triggering offboard")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pubMcsp.publish(pos)
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment