def __init__(self):
# define rate of 10 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]
# mavros velocity publisher
self.mavros_vel_pub = rospy.Publisher("/mavros/rc/out",
RCOut,
queue_size=1)
self.RCOut_msg = RCOut()
######################################################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.robot_roll = 0
self.robot_pitch = 0
self.robot_yaw = 0
# create subscriber for mavros/waypointlist
self.sub_waypoint_list = rospy.Subscriber(
'/mavros/mission/waypoints',
WaypointList,
self.process_waypoint_message,
queue_size=1)
self.list_points = []
self.is_waypoint_loaded = False
self.current_target = [0, 0, 0]
# create subscriber for the state of the machine
self.sub_state = rospy.Subscriber('robdos_sim/state',
state,
self.process_state_message,
queue_size=1)
self.stateCurrent = 2
# create subscriber in order to have the same output in the teleoperation mode
self.sub_rc = rospy.Subscriber('/mavros/rc/out_mavros',
RCOut,
self.process_rc_message,
queue_size=1)
self.RCOut_msgCurrent = RCOut()
######################################################CONTROLLER########################################################
# infinity loop
while not rospy.is_shutdown():
self.update_controller()
self.rate.sleep()
def init_ROS(self):
self.pub_attmsg = rospy.Publisher('/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10)
self.pub_posmsg = rospy.Publisher('/mavros/setpoint_position/local', PoseStamped, queue_size=10)
self.pub_rl = rospy.Publisher('/rl/training', StateReward, queue_size=10)
rospy.init_node('controller_bintel', anonymous=True)
self.rate = rospy.Rate(self.main_loop_rate)
# - Subscribe to local position
self.local_pose = PoseStamped()
self.velocity = TwistStamped()
self.rc_out = RCOut()
pos_sub = message_filters.Subscriber('/mavros/local_position/pose', PoseStamped)
#rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._read_position)
if self.is_simulation:
#rospy.Subscriber('/mavros/local_position/velocity_body', TwistStamped, self._read_velocity)
vel_sub = message_filters.Subscriber('/mavros/local_position/velocity_body', TwistStamped)
else:
#rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, self._read_velocity)
vel_sub = message_filters.Subscriber('/mavros/local_position/velocity', TwistStamped)
ts = message_filters.TimeSynchronizer([pos_sub, vel_sub], 1)
ts.registerCallback(self._read_pos_vel)
# Subscribe to thrust commands from RL
rospy.Subscriber('/rl/commands', AttitudeTarget, self._read_rl_commands)
def init_ROS(self):
rospy.init_node('squidPX4manager', anonymous=True)
self.pub_sp = rospy.Publisher('/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
self.rate = rospy.Rate(self.main_loop_rate)
# - Subscribe to local position
self.local_pose = PoseStamped()
self.velocity = TwistStamped()
self.rc_out = RCOut()
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self._read_position)
if self.is_simulation:
rospy.Subscriber('/mavros/local_position/velocity_body',
TwistStamped, self._read_velocity)
else:
rospy.Subscriber('/mavros/local_position/velocity', TwistStamped,
self._read_velocity)
#rospy.Subscriber('/rovio/pose', PoseStamped, self.poseCallback)
#rospy.Subscriber('/teraranger/distance',Lidar,self.readTeraranger)
rospy.Subscriber('/mavros/rc/out', RCOut, self._read_rc_out)
rospy.wait_for_service('mavros/set_mode')
self.change_mode = rospy.ServiceProxy('mavros/set_mode', SetMode)
def init_ROS(self):
self.pub_sp = rospy.Publisher('/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
self.pub_traj = rospy.Publisher('/mavros/setpoint_raw/trajectory',
PoseStamped,
queue_size=10)
self.pub_force = rospy.Publisher('/bintel/desired_force',
Vector3Stamped,
queue_size=10)
self.desired_path_pub = rospy.Publisher('/desired_path',
Path,
queue_size=10)
rospy.init_node('controller_bintel', anonymous=True)
self.rate = rospy.Rate(self.main_loop_rate)
# - Subscribe to local position
self.local_pose = PoseStamped()
self.velocity = TwistStamped()
self.rc_out = RCOut()
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self._read_position)
if self.is_simulation:
rospy.Subscriber('/mavros/local_position/velocity_body',
TwistStamped, self._read_velocity)
else:
rospy.Subscriber('/mavros/local_position/velocity', TwistStamped,
self._read_velocity)
rospy.Subscriber('/mavros/rc/out', RCOut, self._read_rc_out)
def __init__(self):
# mavros velocity publisher
self.mavros_vel_pub = rospy.Publisher("/mavros/rc/out",
RCOut,
queue_size=1)
self.RCOut_msg = RCOut()
self.RCOR_subscriber = rospy.Subscriber('/mavros/rc/override',
OverrideRCIn,
self.override_cb,
queue_size=1)
def override_cb(self, or_msg):
self.RCOut_msg = RCOut()
dif = or_msg.channels[3] - 1500
if dif >= 0:
K_RM = or_msg.channels[5] + dif
K_LM = or_msg.channels[5] - dif
else:
K_RM = or_msg.channels[5] - abs(dif)
K_LM = or_msg.channels[5] + abs(dif)
K_RM = self.doSymmetric(K_RM)
K_LM = self.doSymmetric(K_LM)
self.RCOut_msg.channels = [self.bound_limit(K_LM, 1100, 1900), self.bound_limit(K_RM, 1100, 1900), \
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.mavros_vel_pub.publish(self.RCOut_msg)
def update_controller(self):
if self.stateCurrent == 1:
self.RCOut_msg = RCOut()
self.RCOut_msg.header.stamp = rospy.Time.now()
self.RCOut_msg.header.frame_id = "0"
self.RCOut_msg.channels = RCOut_msgCurrent.channels
elif self.stateCurrent == 2:
if len(self.list_points) > 0:
point = self.list_points[1]
self.current_target = [point[0], point[1], point[2]]
# update controller
desired_yaw = math.atan2(self.current_target[0],
self.current_target[1])
yaw_error = desired_yaw - self.robot_yaw
#if math.fabs(yaw_error) > 1:
if yaw_error > 0.1:
kp = 1000
K_output = -kp * yaw_error
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.RCOut_msg = RCOut()
self.RCOut_msg.header.stamp = rospy.Time.now()
self.RCOut_msg.header.frame_id = "0"
self.RCOut_msg.channels = [
K_LM, K_RM, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
]
else:
distance_x = self.current_target[0] - self.robot_position_x
distance_y = self.current_target[1] - self.robot_position_y
kp = 1000
error = math.sqrt(
math.pow(distance_x, 2.0) + math.pow(distance_y, 2.0))
if distance_y > 0:
if distance_x > 0:
K_output = -kp * error
elif distance_x < 0:
K_output = -kp * error
elif distance_x == 0:
K_output = -kp * error
elif distance_y < 0:
if distance_x > 0:
K_output = kp * error
elif distance_x < 0:
K_output = kp * error
elif distance_x == 0:
K_output = kp * error
elif distance_y == 0:
if distance_x > 0:
K_output = -kp * error
elif distance_x < 0:
K_output = kp * error
elif distance_x == 0:
K_output = 0
K_output = 1500 + K_output
K_output = self.bound_limit(K_output, 1100, 1900)
# set speed thrusters
self.RCOut_msg = RCOut()
self.RCOut_msg.header.stamp = rospy.Time.now()
self.RCOut_msg.header.frame_id = "0"
self.RCOut_msg.channels = [
K_output, K_output, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0
]
self.mavros_vel_pub.publish(self.RCOut_msg)