def rc_off(self):
"""Need to clear rc channel after a goal"""
# reset control values
channels = [1500] * 8
controlout = OverrideRCIn(channels=channels)
self.contolp.publish(controlout)
self.rate.sleep()
controlout = OverrideRCIn(channels=channels)
self.contolp.publish(controlout)
def __init__(self):
# Publisherを作成
self.publisher = rospy.Publisher('/mavros/rc/override',
OverrideRCIn,
queue_size=1)
# messageの型を作成
self.RC_msg = OverrideRCIn()
self.prev_RC_msg = OverrideRCIn()
self.cmd_vel = Twist()
self.prev_odom = Odometry()
self.gain_x = 120
self.gain_theta = 50
def update_controller(self):
if self.debug:
rospy.loginfo('orientation controller')
# update controller
desired_yaw = math.atan2(
self.current_target[1] - self.robot_position_y,
self.current_target[0] - self.robot_position_x)
# update controller (using degrees)
self.controller_pid.SetPoint = desired_yaw
self.controller_pid.update(self.robot_yaw)
self.controller_pid.output = self.bound_limit(
self.controller_pid.output, -1000, 1000)
if abs(self.controller_pid.error) > (self.threshold_orientation *
self.degrees2rad):
K_output = self.controller_pid.output
K_LM = 1500 + K_output
K_RM = 1500 - K_output
K_LM = self.bound_limit(K_LM, 1100, 1900)
K_RM = self.bound_limit(K_RM, 1100, 1900)
# set speed thrusters
self.RCOR_msg = OverrideRCIn()
self.mapOutToOverride(K_LM, K_RM)
self.mavros_vel_pub.publish(self.RCOR_msg)
else:
K_LM = 1500
K_RM = 1500
# set speed thrusters
self.RCOR_msg = OverrideRCIn()
self.mapOutToOverride(K_LM, K_RM)
self.mavros_vel_pub.publish(self.RCOR_msg)
if self.debug:
rospy.loginfo('oriented')
#rospy.loginfo('orientation self.sub_localization.unregister')
self.sub_localization.unregister()
#self.srv.set_service.shutdown("close process")
#time.sleep(0.1)
msg_ = StateEvent()
self.event_msg.cmd = msg_.ORIENTED
self.event_pub.publish(self.event_msg)
def update_controller(self):
if self.debug:
rospy.loginfo('Position controller')
distance_x = self.current_target[0] - self.robot_position_x
distance_y = self.current_target[1] - self.robot_position_y
self.controller_pid.SetPoint = 0.0
# control objective: reduce squared distance to goal position
self.controller_pid.update(
math.sqrt(math.pow(distance_x, 2.0) + math.pow(distance_y, 2.0)))
# estimate correct orientation
desired_yaw = math.atan2(
self.current_target[1] - self.robot_position_y,
self.current_target[0] - self.robot_position_x)
error_orientation = desired_yaw - self.robot_yaw
# compute error in degrees
error_orientation = (error_orientation % 2 *
math.pi) * self.rad2degrees
# if robot is oriented
if abs(error_orientation) <= self.threshold_orientation:
K_output = 1900 # 1500 + self.controller_pid.output
K_output = self.bound_limit(K_output, 1100, 1900)
# set speed thrusters
self.RCOR_msg = OverrideRCIn()
self.RCOR_msg.channels = [0, 0, 0, 1500, 0, K_output, 0, 0]
self.mavros_vel_pub.publish(self.RCOR_msg)
else:
# stop motors
self.RCOR_msg = OverrideRCIn()
self.RCOR_msg.channels = [0, 0, 0, 1500, 0, 1500, 0, 0]
self.mavros_vel_pub.publish(self.RCOR_msg)
if self.debug:
rospy.loginfo('not oriented')
self.sub_localization.unregister()
#time.sleep(0.1)
msg_ = StateEvent()
self.event_msg.cmd = msg_.NOT_ORIENTED
self.event_pub.publish(self.event_msg)
def behavior_loop(self, goal, rc_cb, terminate_cb):
"""callbacks provide rc commands and termination condition"""
rc_out = rc_cb(self, goal)
is_term = terminate_cb(self, goal)
while not is_term:
# timeout termination behvior
if self._as.is_preempt_requested():
self.rc_off()
rospy.loginfo('Timeout preempted')
self._as.set_preempted()
break
controlout = OverrideRCIn(channels=rc_out)
self.contolp.publish(controlout)
# send command feedback
self.send_feedback(goal)
self.rate.sleep()
# set up next loop
is_term = terminate_cb(self, goal)
rc_out = rc_cb(self, goal)
# publish a result message when finished
if is_term:
self.rc_off()
result = MoveRobotResult(actionID=goal.actionID,
end_heading=self.curr_heading,
end_depth=self.curr_depth)
self._as.set_succeeded(result=result)
def send(self, msg):
""" Send messages on chatter topic at regular rate.
Input:
int[8]:
Channel Meaning
1 Pitch
2 Roll
3 Throttle
4 Yaw
5 Forward
6 Lateral
7 Reserved
8 Camera Tilt
9 Lights 1 Level
10 Lights 2 Level
"""
# i = 0
#while (not rospy.is_shutdown()):
# i = i + 1
# #msg.header.seq = i
# #msg.header.stamp = rospy.Time.now()
# self.chatter_pub.publish(msg)
# self.chat_frequency.sleep()
msg = OverrideRCIn(msg)
#while not rospy.is_shutdown():
print(msg)
self.chatter_pub.publish(msg)
def set_rcvel(self, goal):
"""Set a constant velocity to motor"""
xrc_cmd = goal.x_rc_vel
yrc_cmd = goal.y_rc_vel
# check that command velocity is resonable
if xrc_cmd > self.pwm_center + self.xdiffmax \
or xrc_cmd < self.pwm_center - self.xdiffmax:
raise (ValueError('x goal velocity must be between %i and %i' %
(self.pwm_center + self.xdiffmax,
self.pwm_center - self.xdiffmax)))
if yrc_cmd > self.pwm_center + self.ydiffmax \
or yrc_cmd < self.pwm_center - self.ydiffmax:
raise (ValueError('y goal velocity must be between %i and %i' %
(self.pwm_center + self.ydiffmax,
self.pwm_center - self.ydiffmax)))
# send command to RC channel
channels = [1500] * 8
channels[self.xchannel] = xrc_cmd
channels[self.ychannel] = yrc_cmd
controlout = OverrideRCIn(channels=channels)
while True:
if self._as.is_preempt_requested():
rospy.loginfo('RC set preempted')
self._as.set_preempted()
break
self.contolp.publish(controlout)
self.send_feedback(goal)
self.rate.sleep()
def heading_change(self, goal):
"""Proportional control to change heading"""
hdiff = goal.target_heading - self.curr_heading
while not abs(hdiff) < self.heading_tol:
# compute proportional controller output
pout = hdiff * self.heading_p
# limit output if necassary
if abs(pout) > self.heading_pmax:
if pout < 0:
pout = -self.heading_pmax
else:
pout = self.heading_pmax
if self._as.is_preempt_requested():
rospy.loginfo('Turn preempted')
self._as.set_preempted()
is_success = False
break
pout += self.pwm_center
# send command to RC channel
channels = [1500] * 8
channels[self.rchannel] = pout
controlout = OverrideRCIn(channels=channels)
self.contolp.publish(controlout)
# send command feedback
self.send_feedback(goal)
self.rate.sleep()
hdiff = goal.target_heading - self.curr_heading
if abs(hdiff) < self.heading_tol:
self.rc_off()
result = MoveRobotResult(actionID=goal.actionID,
end_heading=goal.target_heading)
self._as.set_succeeded(result=result)
def sonar_callback(sonar1='', sonar2='', sonar3=''):
sonar_ranges = {}
range_max = 5
if sonar1 is not '':
#print('Sonar1: {}'.format(sonar1.range))
range_max = sonar1.max_range
sonar_ranges['sonar1'] = sonar1.range
if sonar2 is not '':
#print('Sonar2: {}'.format(sonar2.range))
sonar_ranges['sonar2'] = sonar2.range
if sonar3 is not '':
#print('Sonar3: {}'.format(sonar3.range))
sonar_ranges['sonar3'] = sonar3.range
# Use obstacle avoidance algorithm
nav_cmds = sonar_avoidance(sonar_ranges, sonar_angles, range_max)
#nav_cmds = test_avoidance(sonar_ranges, range_max)
msg = OverrideRCIn()
msg.channels[0] = nav_cmds['yaw']
msg.channels[1] = 0
msg.channels[2] = nav_cmds['throttle']
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
obstacle_avoidance_cmds_pub.publish(msg)
print(nav_cmds)
def get_control(chrs, frame_id):
debugging = False
msg = OverrideRCIn()
# msg.header.frame_id = "manual_control_frame"
# msg.header.seq = frame_id
#now = rospy.get_rostime()
# msg.header.stamp.secs = now.secs
# msg.header.stamp.nsecs = now.nsecs
#msg.rssi = 0
msg.channels = [1500, 1500, 1100, 1500, 1100, 1100, 1900, 1900]
if 'w' in chrs and 's' not in chrs:
msg.channels[1] = 1575
if 's' in chrs and 'w' not in chrs:
msg.channels[1] = 1425
if 'a' in chrs and 'd' not in chrs:
msg.channels[0] = 1100
if 'd' in chrs and 'a' not in chrs:
msg.channels[0] = 1900
return msg
def talker():
pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10)
r = rospy.Rate(10) #10hz
msg = OverrideRCIn()
start = time.time()
speed = 'NORMAL'
exec_time = 2
flag = True #time flag
if speed == 'SLOW':
msg.channels[throttle_channel] = 1558
elif speed == 'NORMAL':
msg.channels[throttle_channel] = 1565
elif speed == 'FAST':
msg.channels[throttle_channel] = 1570
direction = 'STRAIGHT'
if direction == 'STRAIGHT':
msg.channels[steer_channel] = 1385
elif direction == 'RIGHT':
msg.channels[steer_channel] = 1450
elif direction == 'LEFT':
msg.channels[steer_channel] = 1300
while not rospy.is_shutdown() and flag:
sample_time = time.time()
if ((sample_time - start) > exec_time):
flag = False
rospy.loginfo(msg)
pub.publish(msg)
r.sleep()
def autopilot_abstraction(speed='SLOW', direction='STRAIGHT', exec_time=1):
pub = rospy.Publisher('mavros/rc/override', OverrideRCIn, queue_size=10)
r = rospy.Rate(10) #10hz
msg = OverrideRCIn()
start = time.time()
flag = True #time flag
# Abstract the speed of the Rover
if speed == 'SLOW':
msg.channels[throttle_channel] = 1558
elif speed == 'NORMAL':
msg.channels[throttle_channel] = 1565
elif speed == 'FAST':
msg.channels[throttle_channel] = 1570
# Abstract the direction of the Rover
if direction == 'STRAIGHT':
msg.channels[steer_channel] = 1500
elif direction == 'LEFT':
msg.channels[steer_channel] = 1200
elif direction == 'RIGHT':
msg.channels[steer_channel] = 1800
while not rospy.is_shutdown() and flag:
sample_time = time.time()
if ((sample_time - start) > exec_time):
flag = False
rospy.loginfo(msg)
pub.publish(msg)
r.sleep()
def __init__(self):
self.control_msg_pub = rospy.Publisher('/autonomous/control_msg',
OverrideRCIn,
queue_size=10,
latch=True)
self.twist_sub = rospy.Subscriber('/mobile_base/commands/velocity',
Twist,
self.callback,
queue_size=10)
self.pub_rate = rospy.get_param("pub_rate")
self.rate = rospy.Rate(self.pub_rate)
self.twist_forward_max = rospy.get_param(
"move_base/DWAPlannerROS/max_vel_x")
self.twist_forward_min = rospy.get_param(
"move_base/DWAPlannerROS/min_vel_x")
self.twist_turn_max = rospy.get_param(
"move_base/DWAPlannerROS/max_rot_vel")
self.twist_turn_min = rospy.get_param(
"move_base/DWAPlannerROS/min_rot_vel")
self.thrust_forward_max = rospy.get_param("thrust_forward_max")
self.thrust_forward_min = rospy.get_param("thrust_forward_min")
self.thrust_turnr_max = rospy.get_param("thrust_turnr_max")
self.thrust_turnr_min = rospy.get_param("thrust_turnr_min")
self.thrust_turnl_max = rospy.get_param("thrust_turnl_max")
self.thrust_turnl_min = rospy.get_param("thrust_turnl_min")
self.override_msg = OverrideRCIn()
def service_callback(self,req):
# dictionary to store the index of channel to control the vehicle
# as indicated on the ArduSub website
index = {'l':5,'r':5,'f':4,'b':4}
# dictionary for easy access to the speeds values
speed = {'l':self.left_sway_speed,'r':self.right_sway_speed,'f':self.forward_speed,
'b':self.backward_speed}
location1 = self._dist_tools.get_coor() # storing the start pos of the vehicle
override_msg = OverrideRCIn() #object of the msg type for the RC
goal_distance = req.desired_distance #getting the service request a
current_distance = 0 #initiating the distance with 0 to avoid errors
# moving the vehicle untill it travel the required distance
while (goal_distance != int(current_distance)):
override_msg.channels[index[req.direction]] = speed[req.direction] #setting the speed to the correct RC channel
self.control_msg_pub.publish(override_msg) # publishing the msg to start movement
self.rate.sleep() # making sure to have the same publish rate
location2 = self._dist_tools.get_coor() # update the current location
current_distance = self.calc_distance(location1,location2) # calculate the distance
override_msg.channels[index[req.direction]] = 1500 # set speed to 1500 (stop)
self.control_msg_pub.publish(override_msg) # publish the stop msg
self.rate.sleep()
location2 = self._dist_tools.get_coor() # update location
rospy.loginfo("The vehicle travelled a distance of: %f successfully",self.calc_distance(location1,location2))
return True # return true for success
def __init__(self):
self.overrideRCIn_pub = rospy.Publisher('mavros/rc/override',
OverrideRCIn,
queue_size=1)
self.received_rc_msg = OverrideRCIn()
self.euler_ref_pub = rospy.Publisher('euler_ref',
Vector3,
queue_size=1)
self.euler_ref_msg = Vector3()
self.roll_command = 0.0
self.pitch_command = 0.0
self.yaw_command = 0.0
self.vpc_roll = 0.0
self.vpc_pitch = 0.0
self.motor_velocity = 0.0
self.rate = rospy.get_param("~rate", 50)
rospy.Subscriber('/jeti/mavros/rc/in', RCIn, self.rcin_callback)
rospy.Subscriber('attitude_command',
Float64MultiArray,
self.attitude_command_cb,
queue_size=1)
rospy.Subscriber('mot_vel_ref', Float64, self.mot_vel_ref_cb)
def callback(data):
global num_vision_cones
#Wait to receive genome to start simulation and start sending commands
if start_sim is False:
#print("No genome received yet!")
return None
#partition data ranges into sections
partitioned_vision = partition_vision(data, num_vision_cones, True)
# Use obstacle avoidance algorithm
nav_cmds = check_vision(data, partitioned_vision)
yaw = nav_cmds['yaw']
throttle = nav_cmds['throttle']
#print('Yaw: {} \t Throttle: {}'.format(yaw,throttle))
msg = OverrideRCIn()
msg.channels[0] = yaw
msg.channels[1] = 0
msg.channels[2] = throttle
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
pub.publish(msg)
def sendViaMavlink(data):
throttle = data.linear.x
#x direction of linear velocity
yaw = data.angular.z
#rotation around z. Hopefully also a velocity, and not a setpoint...
# next, we'll publish the actuatorControl message over ROS
command = OverrideRCIn()
if not on:
command.channels = [1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500]
#Make stationary
command.channels = [
65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535
]
#Relenquish control
elif rc and not isSetRc:
isSetRc = True
command.channels = [
65535, 65535, 65535, 65535, 65535, 65535, 65535, 65535
]
else:
isSetRc = False
command.channels = [
1500 + 500 * -yaw, 1500, 1500 + 500 * throttle, 1500, 1500, 1500,
1500, 1500
]
# index 0 is yaw, index 2 is throttle.
# publish the message
p.publish(command)
rospy.loginfo("dyaw: " + str(yaw) + ", dx: " + str(throttle))
def service_callback(self, req):
# dictionary to store the index of channel to control the vehicle
# as indicated on the ArduSub website
index = {
'l': 5,
'r': 5,
'f': 4,
'b': 4,
'rl': 3,
'rr': 3,
'u': 2,
'd': 2,
's': -1,
'pu': 0,
'pd': 0
}
# dictionary for easy access to the speeds values
speed = {
'l': self.left_sway_speed,
'r': self.right_sway_speed,
'f': self.forward_speed,
'b': self.backward_speed,
'rl': self.left_rotation_speed,
'rr': self.right_rotation_speed,
'u': self.up_speed,
'd': self.down_speed,
'pu': self.pitch_up_speed,
'pd': self.pitch_down_speed
}
loginfo_msg = {
'l': "lateral left",
'r': "lateral right",
'f': "forward",
'b': "backward",
'rl': "left roatation",
'rr': "right rotation",
'u': "depth up",
'd': "depth down",
'pu': "pitching up",
'pd': "pitching down"
}
self.update_params()
override_msg = OverrideRCIn() #object of the msg type for the RC
if req.direction in index: #error catcher to verify a valid direction is sent
override_msg.channels = [
1500 for x in range(len(override_msg.channels))
]
if req.direction == 's':
self.control_msg_pub.publish(override_msg)
self.rate.sleep()
rospy.loginfo("vehicle stopped")
return True
override_msg.channels[index[req.direction]] = speed[req.direction]
self.control_msg_pub.publish(override_msg)
self.rate.sleep()
rospy.loginfo("The vehicle started doing a %s",
loginfo_msg[req.direction])
return True
else: #catching the error
rospy.logerr("you have endered wrong direction for move service")
def callback(data):
global num_vision_cones
#partition data ranges into sections
partitioned_vision = partition_vision(data, num_vision_cones, True)
# Use obstacle avoidance algorithm
nav_cmds = check_vision(data, partitioned_vision)
yaw = nav_cmds['yaw']
throttle = nav_cmds['throttle']
#print('Yaw: {} \t Throttle: {}'.format(yaw,throttle))
msg = OverrideRCIn()
msg.channels[0] = yaw
msg.channels[1] = 0
msg.channels[2] = throttle
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
pub.publish(msg)
def depth_change(self, goal):
"""Proportional control to change depth"""
#self.mode_setter(base_mode=0, custom_mode='MANUAL')
target_depth = goal.target_depth
while not abs(target_depth - self.curr_depth) < self.depth_tol:
# compute proportional controller output
if self._as.is_preempt_requested():
rospy.loginfo('Dive preempted')
self._as.set_preempted()
break
# send command to RC channel
channels = [1500] * 8
zout = self._get_depth_pwm(goal.target_depth)
channels[self.zchannel] = zout
controlout = OverrideRCIn(channels=channels)
self.contolp.publish(controlout)
# send command feedback
self.send_feedback(goal)
self.rate.sleep()
# publish a result message when finished
if abs(target_depth - self.curr_depth) < self.depth_tol:
self.rc_off()
result = MoveRobotResult(actionID=goal.actionID,
end_depth=goal.target_depth)
self._as.set_succeeded(result=result)
def heading_change(self, goal):
"""Proportional control to change heading"""
#self.mode_setter(base_mode=0, custom_mode='MANUAL')
target_heading = goal.target_heading
while not abs(target_heading - self.curr_heading) < self.heading_tol:
# compute proportional controller output
if self._as.is_preempt_requested():
rospy.loginfo('Turn preempted')
self._as.set_preempted()
is_success = False
break
# send command to RC channel
channels = [1500] * 8
hout = self._get_heading_pwm(target_heading)
channels[self.rchannel] = hout
controlout = OverrideRCIn(channels=channels)
self.contolp.publish(controlout)
# send command feedback
self.send_feedback(goal)
self.rate.sleep()
if abs(target_heading - self.curr_heading) < self.heading_tol:
self.rc_off()
result = MoveRobotResult(actionID=goal.actionID,
end_heading=goal.target_heading)
self._as.set_succeeded(result=result)
def __init__(self, current_target, debug=False):
self.degrees2rad = math.pi / 180.0
self.rad2degrees = 180.0 / math.pi
self.debug = debug
self.current_target = current_target
# define rate of 100 hz
self.rate = rospy.Rate(50.0)
# init variables for odometry
[self.robot_position_x, self.robot_position_y,
self.robot_position_z] = [0, 0, 0]
[self.robot_roll, self.robot_pitch, self.robot_yaw] = [0, 0, 0]
self.is_positioned = False
self.is_oriented = True
"""initialize controller"""
self.threshold_position = 0.0
self.threshold_orientation = 100 # degree of tolerance
self.S_kp = 1000.0
self.S_kd = 100.0
self.S_ki = 0.0
self.S_windup = 0.0
self.controller_pid = PID()
self.controller_pid.SetPoint = 0.0
self.controller_pid.setSampleTime(0.01)
self.controller_pid.setKp(self.S_kp)
self.controller_pid.setKd(self.S_kd)
self.controller_pid.setKi(self.S_ki)
self.controller_pid.setWindup(self.S_windup)
# event publisher
self.event_pub = rospy.Publisher("/robdos/stateEvents",
StateEvent,
queue_size=1)
self.event_msg = StateEvent()
# mavros velocity publisher
self.mavros_vel_pub = rospy.Publisher("/mavros/rc/override",
OverrideRCIn,
queue_size=1)
self.RCOR_msg = OverrideRCIn()
######################################################SUBSCRIBERS########################################################
# create subscriber for robot localization
# self.sub_localization = rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self.process_localization_message, queue_size=1)
self.sub_localization = rospy.Subscriber(
'/robdos/odom',
Odometry,
self.process_localization_message,
queue_size=1)
# self.sub_localization = rospy.Subscriber('/mavros/global_position/local', Odometry, self.process_localization_message, queue_size=1)
self.sub_waypoint_list = rospy.Subscriber(
'/mavros/mission/waypoints',
WaypointList,
self.process_waypoint_message,
queue_size=1)
def __init__(self):
# Set up a subscriber that will act as the trigger mechanism
# Use a std_msgs/Bool to allow for open/close
self.sub_deploy = rospy.Subscriber('~deploy_2', Bool,
self.callback_deploy)
# Set the PWM Override message so we don't have to re-compile it every time
# By default, set it so that none of the other servo outputs are overriden
self.msg_out = OverrideRCIn()
for ch in self.msg_out.channels:
ch = self.msg_out.CHAN_NOCHANGE
# Read in what servo we want to control, and it's open/close settings
self.servo_ch = int(rospy.get_param("~servo_ch"))
self.servo_low = int(rospy.get_param("~servo_low"))
self.servo_high = int(rospy.get_param("~servo_high"))
# Set the starting value for the servo
servo_start_high = bool(rospy.get_param("~servo_start_high"))
if servo_start_high:
self.servo_value = self.servo_high
else:
self.servo_value = self.servo_low
# Set up the publisher that will send the output override commands
self.pub_override = rospy.Publisher('~override_2',
OverrideRCIn,
queue_size=10)
# Set up a timer to send PWM commands to the autopilot
# By default, robin expects a command stream at a 5Hz, so lets double that
self.timer = rospy.Timer(rospy.Duration(0.1), self.callback_pwm_out)
def move_forward():
global msg,SPEED,tt,ST,turn_plus
if ST:
tt = time.time()
ST = False
# if time.time() - tt >= 10:
# tt = time.time()
# SPEED = SPEED + 8
msg = OverrideRCIn()
robot_speed = SPEED + vel_ctrl_effort_scale * vel_ctrl_effort
if robot_speed > MAX_SPEED:
robot_speed = MAX_SPEED
if robot_speed < MIN_SPEED:
robot_speed = MIN_SPEED
msg.channels[throttle] = int(robot_speed)
turn_amt = 1500 + hdg_ctrl_effort_scale*hdg_ctrl_effort
if turn_amt > MAX_TURN:
turn_amt = MAX_TURN
elif turn_amt < MIN_TURN:
turn_amt = MIN_TURN
msg.channels[steer] = int(turn_amt)
def roll():
rcin = OverrideRCIn()
for i in range (0, 8): rcin.channels[i] = 1500
rcin.channels[0] = 1400
rcin.channels[1] = 1600
rospy.loginfo("Roll")
motor_pub.publish(rcin)
def sonar_callback(sonar1='',
sonar2='',
sonar3='',
sonar4='',
sonar5='',
sonar6='',
sonar7='',
sonar8='',
sonar9='',
sonar10=''):
global vehicle
global last_vehicle_mode
global history_queue
global last_heading
ang = angle_to_current_waypoint(vehicle)
#print('Bearing: {} \t Heading: {}'.format(ang, last_heading))
sonar_ranges = {}
range_max = 2.5
hybrid_zone_cutoff = 2
# Read any sonar data available and put it into a single dict called sonar_ranges
# Also have a modifier for range_max - This is in case the max sensing capabilities are changed in the URDF file
# It is -0.5 because at the default angle that sonars are passed on the rover, they catch the ground at the very end
# of the sensing range. We want to ignore the ground readings while still picking up small objects
for j in range(1, 11):
current_sonar = 'sonar' + str(j)
if eval(current_sonar) is not '':
range_max = eval(current_sonar).max_range - 0.5
sonar_ranges[current_sonar] = eval(current_sonar).range
# Check to see if an object is in the path of the rover
if all(i >= range_max for i in sonar_ranges.values()):
# All is good
if (vehicle.mode == VehicleMode("MANUAL")):
#print('Clear of obstacle! Switching to {}'.format(last_vehicle_mode))
vehicle.mode = last_vehicle_mode
else:
if (vehicle.mode != VehicleMode("MANUAL")):
#print('Object detected! Obstacle avoidance engaged!')
last_vehicle_mode = vehicle.mode
vehicle.mode = VehicleMode("MANUAL")
# Use obstacle avoidance algorithm
nav_cmds = sonar_avoidance(sonar_ranges, sonar_angles, range_max)
msg = OverrideRCIn()
msg.channels[0] = nav_cmds['yaw']
msg.channels[1] = 0
msg.channels[2] = nav_cmds['throttle']
msg.channels[3] = 0
msg.channels[4] = 0
msg.channels[5] = 0
msg.channels[6] = 0
msg.channels[7] = 0
obstacle_avoidance_cmds_pub.publish(msg)
def RCInOverride(channel0,channel1,channel2,channel3): target_RC_yaw = OverrideRCIn() target_RC_yaw.channels[0] = channel0 target_RC_yaw.channels[1] = channel1 target_RC_yaw.channels[2] = channel2 target_RC_yaw.channels[3] = channel3 target_RC_yaw.channels[4] = 1100 return target_RC_yaw
def gate_pass(self, goal):
"""Pass through the gate"""
target_depth = goal.target_depth
target_heading = goal.target_heading
xrc_cmd = goal.x_rc_vel
isclose = False # if true just go for the gate
count = 0
# check that command velocity is resonable
if xrc_cmd > self.pwm_center + self.xdiffmax \
or xrc_cmd < self.pwm_center - self.xdiffmax:
raise(ValueError('x goal velocity must be between %i and %i'%(
self.pwm_center + self.xdiffmax,
self.pwm_center - self.xdiffmax)))
if yrc_cmd > self.pwm_center + self.ydiffmax \
or yrc_cmd < self.pwm_center - self.ydiffmax:
raise(ValueError('y goal velocity must be between %i and %i'%(
self.pwm_center + self.ydiffmax,
self.pwm_center - self.ydiffmax)))
# send command to RC channel
channels = [1500] * 8
channels[self.xchannel] = xrc_cmd
while True:
if self._as.is_preempt_requested():
self.rc_off()
rospy.loginfo('Gate pass preempted')
self._as.set_preempted()
break
# calculate heading and depth
zout = self._get_depth_pwm(target_depth)
hout = self._get_heading_pwm(target_heading)
channels[self.zchannel] = zout
channels[self.rchannel] = hout
if self.object_width > self.maxwidth:
isclose = True
if not isclose:
# center on the object
yrc_cmd = self._get_obj_pwm()
channels[self.ychannel] = yrc_cmd
else:
channels[self.ychannel] = 1500
count += 1
if count > self.objcycles:
break
controlout = OverrideRCIn(channels=channels)
self.contolp.publish(controlout)
self.send_feedback(goal)
self.rate.sleep()
def __init__(self,controller,view):
"""
The class for the MAVROS Module
"""
print("Hello I am in Mavros Now")
"""
In this class following things are required
1. Subscriber for Compass heading
2. Subscriber for Local_X
3. Subscriber for Local_Y
4. Publisher for RC_commands.
5. Definition of RC_channel_msgs
6. Add service for Arm and Disarm commands.
7. Controller object
"""
self.view = view
self.compass_subscriber = rospy.Subscriber("/mavros/global_position/compass_hdg",Float64,self.compass_callback)
"""
# this was the previous fused Kalman filter data which proved to be unreliable. Now we are using
the global gps values as written in the subsequent lines.
# self.location_X_subscriber = rospy.Subscriber("/mavros/local_position/pose",PoseStamped,self.local_x_callback)
# self.location_Y_subscriber = rospy.Subscriber("/mavros/local_position/pose",PoseStamped,self.local_y_callback)
"""
self.location_X_subscriber = rospy.Subscriber("/mavros/global_position/local",Odometry,self.local_x_callback)
self.location_Y_subscriber = rospy.Subscriber("/mavros/global_position/local",Odometry,self.local_y_callback)
self.rc_message = OverrideRCIn()
self.rc_message.channels = [0,0,0,0,0,0,0,982] #[1500,1500,1500,1500,0,0,1500,982]
self.RC_Publisher = rospy.Publisher("/mavros/rc/override",OverrideRCIn,queue_size=10)
self.DP_compass_flag = 0 #flag to turn on/off compass_dp,main flag
self.DP_local_x_flag = 0 #flag to turn on/off local_x_dp,main flag
self.DP_local_y_flag = 0 #flag to turn on/off local_y_dp,main flag
self.safe_pwm = 1495
self.highband = 1550
self.lowband = 1440
self.controller = controller #passing the controler object
self.compass_value_placeholder = []
self.compass_value_SP_placeholder = []
self.local_x_placeholder = []
self.local_x_value_SP_placeholder = []
self.local_y_placeholder = []
self.local_y_value_SP_placeholder = []
self.local_x_value = 0 #placeholder to update the subscribed x values
self.local_y_value = 0 #placeholder to update the subscribed y values
self.arm_disarm = rospy.ServiceProxy('/mavros/cmd/arming',mavros_msgs.srv.CommandBool)
self.animation_main_for_matplot()
self.dp_margin = 0.3
"""Experimental--almost approved"""
self.counter_local_x = 0 #internal counter for local X
self.counter_local_y = 0 #internal counter for local Y
self.internal_local_x_dp_flag = 0 #internal flag
self.internal_local_y_dp_flag = 0 #internal flag
self.local_x_pwm = self.safe_pwm
self.local_y_pwm = self.safe_pwm
self.compass_pwm_out = self.safe_pwm
"""
def mundur():
rcin = OverrideRCIn()
for i in range (0, 8): rcin.channels[i] = 1500
rcin.channels[2] = 1450
rcin.channels[3] = 1450
rcin.channels[4] = 1450
rcin.channels[5] = 1450
rospy.loginfo("Mundur")
motor_pub.publish(rcin)
