def main():
rospy.init_node('actuator_controller', anonymous=True)
rospy.Subscriber("/release_signal", String, callback)
pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10)
rate = rospy.Duration(1/5.0)
msg_out = ActuatorControl()
msg_out.group_mix = 2 # Use group 2 (auxilary controls)
msg_out.controls = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
if blue_open:
act_index = blue_servo - 1
msg_out.controls[act_index] = 1.0
else:
act_index = blue_servo - 1
msg_out.controls[act_index] = -1.0
if orange_open:
act_index = orange_servo - 1
msg_out.controls[act_index] = 1.0
else:
act_index = orange_servo - 1
msg_out.controls[act_index] = -1.0
msg_out.header.stamp = rospy.Time.now()
pub.publish(msg_out)
rospy.sleep(rate)
def talker():
rospy.init_node('actuator_controller', anonymous=True)
sub_trig = rospy.Subscriber('~imagery_trigger', Empty, talker)
pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10)
rate = rospy.Rate(1)
msg_out = ActuatorControl()
msg_out.group_mix = 1 # Use group 1 (auxilary controls)
msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0]
is_low = True
if not rospy.is_shutdown():
is_low = not is_low
if is_low:
msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0]
rospy.loginfo("Set servos low")
else:
msg_out.controls = [0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0]
rospy.loginfo("Set servos high")
msg_out.header.stamp = rospy.Time.now()
pub.publish(msg_out)
rate.sleep()
def pitch_camera(self):
pitch_actuator = ActuatorControl()
pitch_actuator.group_mix = 2
pitch_actuator.controls[1] = -0.7
import pdb
pdb.set_trace()
for jj in range(20):
self.uav_actuator.publish(pitch_actuator)
self.uav_set_mode(base_mode=0, custom_mode="OFFBOARD")
self.uav_arming(True)
self.r.sleep()
def motr(a):
rospy.init_node('motor', anonymous=True)
pub = rospy.Publisher("/mavros/actuator_control",
ActuatorControl,
queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
msg = ActuatorControl()
msg.header.frame_id = "motorcontrol"
msg.controls = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
msg.group_mix = 3
time.sleep(3)
print 1
pub.publish(msg)
time.sleep(3)
rospy.spin()
def img_callback(img_msg):
# publish actuator steering with inference result if in OFFBOARD MODE
global offboard_mode_is_active
if offboard_mode_is_active:
global model
global graph
global pub_act_ctl
# preprocess img as in ML pipeline with PIL
# img = np.array(Image.frombytes('RGB',(160,120),io.BytesIO(img_msg.data),'raw'))
img = np.array(Image.open(io.BytesIO(img_msg.data)))
# img_arr = np.frombuffer(img_msg.data, dtype='int8').reshape(160, 120, 3)
# img inference
#deb = rospy.Time.now()
with graph.as_default():
#probs = model.predict(img_arr.reshape((1,) + img_arr.shape))
probs = model.predict(img.reshape((1, ) + img.shape))
#print(rospy.Time.now()-deb)
# process classes to output [-1,1] from 15 classes array
bucket_no = np.argmax(probs)
act_val = (bucket_no / 7) - 1.
# publish value in actuator_output_msg.channels[1] and fix power in actuator_output_msg.channels[1]
actuator_control = ActuatorControl()
actuator_control.header.stamp = rospy.Time.now()
actuator_control.group_mix = 0
actuator_control.controls[0] = 0.0
actuator_control.controls[1] = 0.0
actuator_control.controls[2] = act_val
actuator_control.controls[3] = 0.2 # Valeur entre 0 et 1
actuator_control.controls[4] = 0.0
actuator_control.controls[5] = 0.0
actuator_control.controls[6] = 0.0
actuator_control.controls[7] = 0.0
pub_act_ctl.publish(actuator_control)
def actuator_control(U):
actuator_publisher = rospy.Publisher('/mavros/actuator_control',
ActuatorControl,
queue_size=10)
act_msg = ActuatorControl()
rate = rospy.Rate(100) # 10hz
act_msg.controls[0] = 0.0
act_msg.controls[1] = 0.0
act_msg.controls[2] = 0.0
act_msg.controls[3] = 0.0
act_msg.controls[4] = 0.0
act_msg.controls[5] = 0.0
act_msg.controls[6] = 0.0
act_msg.controls[7] = 0.0
if U[0] > 0.7:
U[0] = 0.7
act_msg.header.stamp = rospy.Time.now()
act_msg.group_mix = 0
if not U[0] == 0 or not U[1] == 0:
act_msg.controls[0] = U[1] - 0.2
act_msg.controls[3] = U[0]
else:
act_msg.controls[0] = 0.0
act_msg.controls[3] = 0.0
act_msg.controls[1] = 0.0
act_msg.controls[2] = 0.0
#act_msg.controls[3] = 0.5
act_msg.controls[4] = 0.0
act_msg.controls[5] = 0.0
act_msg.controls[6] = 0.0
act_msg.controls[7] = 0.0
actuator_publisher.publish(act_msg)
rate.sleep()
def deploy_blue(self):
pub = rospy.Publisher('/mavros/actuator_control', ActuatorControl, queue_size=10)
rate = rospy.Rate(1)
msg_out = ActuatorControl()
msg_out.group_mix = 1 # Use group 1 (auxilary controls)
msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0]
is_low = True
if not rospy.is_shutdown():
is_low = not is_low
if is_low:
msg_out.controls = [0.0, 0.0, 0.0, 0.0, -1.0, -1.0, -1.0, -1.0]
rospy.loginfo("Set servos low")
else:
msg_out.controls = [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 1.0]
rospy.loginfo("Set servos high")
msg_out.header.stamp = rospy.Time.now()
pub.publish(msg_out)
def set_act(self, group, output, freq):
"""
Offboard method that sets the values of the mixers and/or actuators of the vehicle.
Parameters
----------
group: int
Desired control group.
output : list
Desired output values for the mixers and/or actuators of the drone.
freq : float
Topic publishing frequency, in Hz.
"""
pub = rospy.Publisher(self.info.drone_ns + '/mavros/actuator_control',
ActuatorControl,
queue_size=1)
msg = ActuatorControl()
msg.header = Header()
msg.header.stamp = rospy.Time.now()
msg.group_mix = group
msg.controls = output
pub.publish(msg)
rate = rospy.Rate(freq)
rate.sleep()
def listener():
# In ROS, nodes are uniquely named. If two nodes with the same
# node are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
rospy.init_node('listener', anonymous=True)
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch = roslaunch.parent.ROSLaunchParent(
uuid, ["/home/indigo/src/Firmware/launch/px4_posix_sitl.launch"])
launch.start()
pub = rospy.Publisher('/mavros/actuator_control',
ActuatorControl,
queue_size=10)
rate = rospy.Rate(200) # 10hz
set_offboard_mode()
# next_step()
# next_step()
delay()
arm_quad()
# next_step()
# next_step()
while not rospy.is_shutdown():
# delay()
actuatorControl = ActuatorControl()
actuatorControl.group_mix = actuatorControl.PX4_MIX_MANUAL_PASSTHROUGH
actuatorControl.controls = [0, 0, 0, 1, 0, 0, 0, 0]
pub.publish(actuatorControl)
# next_step()
rate.sleep()
queue_size=1)
self.servo_state = 0
if __name__ == '__main__':
rospy.init_node('release_servo_test_node', anonymous=False)
rate = rospy.Rate(1) # 10hz
n = release_test()
n.servo_state = 0
cmd = ActuatorControl()
while not rospy.is_shutdown():
if n.servo_state == 0:
cmd.group_mix = 1
cmd.controls[4] = 1
cmd.controls[5] = 1
n.cmd_pub.publish(cmd)
n.servo_state = 1
else:
cmd.group_mix = 1
cmd.controls[4] = -1
cmd.controls[5] = -1
n.cmd_pub.publish(cmd)
n.servo_state = 0
print(n.servo_state)
rate.sleep()
rospy.spin()
#arm_cmd = CommandBool()
#arm_cmd.request.value = True
#mode_cmd = SetMode()
#mode_cmd
# Prepare dummy pose sp
#send a few setpoints before starting
#pList = 10*[PoseStamped()]
pose_sp = PoseStamped()
pose_sp.pose.position.x = 5
pose_sp.pose.position.y = 2
pose_sp.pose.position.z = 2
actuator_sp = ActuatorControl()
actuator_sp.controls[5] = 0.5
actuator_sp.group_mix = ActuatorControl.PX4_MIX_MANUAL_PASSTHROUGH
i = 50
while (not rospy.is_shutdown()) and i > 0:
pub_pose_sp.publish(pose_sp)
i = i - 1
rate.sleep()
# Loop until ROS Shutdown
last_request = time.time()
while not rospy.is_shutdown():
#rospy.loginfo('state: ' + cur_state.mode)
if (cur_state.mode != "OFFBOARD" and (time.time() - last_request > 1)):
rospy.loginfo('attempting OFFBOARD')
success = setMode(custom_mode="OFFBOARD")
if (success and True): # offb_set_mode.response.mode_sent){
