def __init__(self, root):
rospy.init_node('tello_ui', anonymous=False)
# rospy.Subscriber('/command_pos', Point, self.command_pos_callback)
signal.signal(signal.SIGINT, self.onClose)
signal.signal(signal.SIGTERM, self.onClose)
try:
self.id = rospy.get_param('~ID')
except KeyError:
self.id = 0
self.publish_prefix = "tello{}/".format(self.id)
self.point_command_pos = Point(0.0, 0.0, 1.0)
self.point_command_pos_yaw = 0.0
self.command_pos = Pose()
self.rotated_pos = Point()
self.slam_pos = Point()
self.twist_manual_control = Twist()
self.real_world_pos = Point()
self.delta_pos = Point()
self.orientation_degree = Point()
self.allow_slam_control = False
self.current_mux = 0
# initialize the root window and image panel
self.root = root
self.panel = None
# self.panel_for_pose_handle_show = None
# self.client = dynamic_reconfigure.client.Client("orb_slam2_mono")
# create buttons
self.column = 0
self.row = 0
self.frame_column = 0
self.frame_row = 0
self.angle_delta_x = 0
self.angle_delta_y = 0
self.angle_delta_z = 0
self.angle = 12.0
self.angle_radian = self.angle / 180.0 * math.pi
self.real_world_scale = 3.9636
self.altitude = 0
self.init_command_pos_frame_flag = False
self.init_real_world_frame_flag = False
self.init_slam_pose_frame_flag = False
self.init_delta_frame_flag = False
self.init_speed_frame_flag = False
self.init_info_frame_flag = False
self.init_manual_control_frame_flag = False
self.init_angle_calc_frame_flag = False
self.init_rotated_frame_flag = False
self.init_command_pos_frame()
self.init_real_world_frame()
self.init_rotated_frame()
# self.init_slam_pose_frame()
self.init_delta_frame()
self.init_speed_frame()
self.init_info_frame()
self.init_manual_control_frame()
# self.init_angle_calc_frame()
self.init_kd_kp_frame()
self.update_command_pos_to_gui()
self.row += 1
self.column = 0
# set a callback to handle when the window is closed
self.root.wm_title("TELLO Controller")
self.root.wm_protocol("WM_DELETE_WINDOW", self.onClose)
self.kd = Pose()
self.kp = Pose()
rospy.Subscriber('/orb_slam2_mono/pose', PoseStamped, self.slam_callback)
rospy.Subscriber(self.publish_prefix+'delta_pos', Point, self.delta_pos_callback)
rospy.Subscriber(self.publish_prefix+'cmd_vel', Twist, self.speed_callback)
rospy.Subscriber(self.publish_prefix+'flight_data', FlightData, self.flightdata_callback)
rospy.Subscriber(self.publish_prefix+'allow_slam_control', Bool, self.allow_slam_control_callback)
rospy.Subscriber(self.publish_prefix+'real_world_scale', Float32, self.real_world_scale_callback)
rospy.Subscriber(self.publish_prefix+'real_world_pos', PoseStamped, self.real_world_pos_callback)
rospy.Subscriber(self.publish_prefix+'rotated_pos', Point, self.rotated_pos_callback)
rospy.Subscriber(self.publish_prefix+'command_pos', Pose, self.command_pos_callback)
rospy.Subscriber(self.publish_prefix+'orientation', Point, self.orientation_callback)
self.command_pos_publisher = rospy.Publisher(self.publish_prefix+'command_pos', Pose, queue_size = 1)
self.pub_takeoff = rospy.Publisher(self.publish_prefix+'takeoff', Empty, queue_size=1)
self.pub_land = rospy.Publisher(self.publish_prefix+'land', Empty, queue_size=1)
self.pub_allow_slam_control = rospy.Publisher(self.publish_prefix+'allow_slam_control', Bool, queue_size=1)
self.cmd_val_publisher = rospy.Publisher(self.publish_prefix+'cmd_vel', Twist, queue_size = 1)
self.calibrate_real_world_scale_publisher = rospy.Publisher(self.publish_prefix+'calibrate_real_world_scale', Empty, queue_size = 1)
self.scan_room_publisher = rospy.Publisher(self.publish_prefix+'scan_room', Bool, queue_size = 1)
self.kd_publisher = rospy.Publisher(self.publish_prefix+'kd', Pose, queue_size = 1)
self.kp_publisher = rospy.Publisher(self.publish_prefix+'kp', Pose, queue_size = 1)
self.kp_publisher = rospy.Publisher(self.publish_prefix+'kp', Pose, queue_size = 1)
self.pub_mux = rospy.Publisher('tello_mux', Int32, queue_size = 1)
self.publish_command()
def listen(self):
self.mag_sub = rospy.Subscriber("/compass", Float32, self.mag_callback)
self.base_sub = rospy.Subscriber("/base_gps_data", NavSatFix, self.base_callback)
self.rover_sub = rospy.Subscriber("/rover_gps_data", NavSatFix, self.rover_callback)
self.mode_sub = rospy.Subscriber("/mode", String, self.mode_callback)
self.atmo_sub = rospy.Subscriber("/pressure", Float32, self.atmo_callback)
self.rad_sub = rospy.Subscriber("/radiation", Float32, self.rad_callback)
self.rad_error_sub = rospy.Subscriber("/radiation_error", Float32, self.rad_error_callback)
self.temp_sub = rospy.Subscriber("/temperature", Float32, self.temperature_callback)
self.radio_sub = rospy.Subscriber("/radio", Float32, self.radio_callback)
self.oxygen_sub = rospy.Subscriber("/oxygen", Float32, self.oxygen_callback)
self.load_sub = rospy.Subscriber("/load", Float32, self.load_callback)
pass
pose = msg.pose.pose
rospy.init_node("setpoint_visiter", anonymous=True)
# if actor == "red_actor_5":
# rospy.Subscriber("{}/pose_gt".format(actor), Odometry, callback)
# rospy.wait_for_message("{}/pose_gt".format(actor), Odometry)
# else:
# rospy.Subscriber("{}/strapdown/estimate".format(actor), Odometry, callback)
# rospy.wait_for_message("{}/strapdown/estimate".format(actor), Odometry)
# print("Odom")
# rospy.Subscriber("{}/odometry/filtered/odom".format(actor), Odometry, callback)
# rospy.wait_for_message("{}/odometry/filtered/odom".format(actor), Odometry)
# print("Received msg")
rospy.Subscriber("{}/pose_gt".format(actor), Odometry, callback)
rospy.wait_for_message("{}/pose_gt".format(actor), Odometry)
print("rosservice call /{}/uuv_control/arm_control ".format(actor) + "{}")
os.system("rosservice call /{}/uuv_control/arm_control ".format(actor) + "{}")
print("armed...")
current_index = 0
# ENU setpoints
# setpoints = [[0,3],[3,3],[0,3],[0,0]]
# setpoints = [[5,0],[5,5],[0,5],[0,0]]
# setpoints = [[7,0],[0,0]]
heading_setpoint = 90.0
depth_setpoint = 0.5
def __init__(self):
self.node_name = "Lane Filter"
self.active = True
self.updateParams(None)
self.d, self.phi = np.mgrid[self.d_min:self.d_max:self.delta_d,
self.phi_min:self.phi_max:self.delta_phi]
self.beliefRV = np.empty(self.d.shape)
self.initializeBelief()
self.lanePose = LanePose()
self.lanePose.d = self.mean_0[0]
self.lanePose.phi = self.mean_0[1]
self.dwa = -(self.zero_val * self.l_peak**2 + self.zero_val *
self.l_max**2 - self.l_max**2 * self.peak_val -
2 * self.zero_val * self.l_peak * self.l_max +
2 * self.l_peak * self.l_max * self.peak_val) / (
self.l_peak**2 * self.l_max *
(self.l_peak - self.l_max)**2)
self.dwb = (2 * self.zero_val * self.l_peak**3 + self.zero_val *
self.l_max**3 - self.l_max**3 * self.peak_val -
3 * self.zero_val * self.l_peak**2 * self.l_max +
3 * self.l_peak**2 * self.l_max * self.peak_val) / (
self.l_peak**2 * self.l_max *
(self.l_peak - self.l_max)**2)
self.dwc = -(self.zero_val * self.l_peak**3 + 2 * self.zero_val *
self.l_max**3 - 2 * self.l_max**3 * self.peak_val -
3 * self.zero_val * self.l_peak * self.l_max**2 +
3 * self.l_peak * self.l_max**2 * self.peak_val) / (
self.l_peak * self.l_max *
(self.l_peak - self.l_max)**2)
self.t_last_update = rospy.get_time()
self.v_current = 0
self.w_current = 0
self.v_last = 0
self.w_last = 0
self.v_avg = 0
self.w_avg = 0
# Subscribers
if self.use_propagation:
self.sub_velocity = rospy.Subscriber("/lane_filter_node/velocity",
Twist2DStamped,
self.updateVelocity)
self.sub = rospy.Subscriber("~segment_list",
SegmentList,
self.processSegments,
queue_size=1)
# Publishers
self.pub_lane_pose = rospy.Publisher("~lane_pose",
LanePose,
queue_size=1)
self.pub_belief_img = rospy.Publisher("~belief_img",
Image,
queue_size=1)
self.pub_entropy = rospy.Publisher("~entropy", Float32, queue_size=1)
#self.pub_prop_img = rospy.Publisher("~prop_img", Image, queue_size=1)
self.pub_in_lane = rospy.Publisher("~in_lane",
BoolStamped,
queue_size=1)
self.sub_switch = rospy.Subscriber("~switch",
BoolStamped,
self.cbSwitch,
queue_size=1)
self.timer = rospy.Timer(rospy.Duration.from_sec(1.0),
self.updateParams)
from std_msgs.msg import String, Float32, Bool, Float32MultiArray
from sensor_msgs.msg import JointState
from kinova_msgs.msg import JointVelocity
torque = 0
#define function/functions to provide the required functionaI needlity
def fnc_callbacktool(msg):
global torque
torque = msg.effort[0]
if __name__=='__main__':
#Add here the name of the ROS. In ROS, names are unique named.
rospy.init_node('example')
#subscribe to a topic using rospy.Subscriber class
subtool=rospy.Subscriber('/j2n6s300_driver/out/joint_state', JointState, fnc_callbacktool)
pub=rospy.Publisher('j2n6s300_driver/in/joint_velocity', JointVelocity, queue_size=10)
rate=rospy.Rate(100)
while not rospy.is_shutdown():
cmd = JointVelocity()
if torque > 0.3:
cmd.joint6=48
elif torque < -0.3:
cmd.joint6=-48
else:
cmd.joint6=0
print cmd
print torque
def ros2zmq():
global g_socket, g_lock
with g_lock:
context = zmq.Context()
g_socket = context.socket(zmq.PUSH)
g_socket.setsockopt(zmq.LINGER, 100) # milliseconds
g_socket.bind('tcp://*:5555')
context2 = zmq.Context()
g_socket2 = context2.socket(zmq.PULL)
g_socket2.RCVTIMEO = 5000 # in milliseconds
g_socket2.bind('tcp://*:5556')
rospy.init_node('listener', anonymous=True)
rospy.Subscriber('/scout_1/joint_states', JointState, callback_topic, '/scout_1/joint_states')
rospy.Subscriber('/scout_1/laser/scan', LaserScan, callback_lidar)
rospy.Subscriber('/scout_1/imu', Imu, callback_imu)
lights_on = rospy.ServiceProxy('/scout_1/toggle_light', ToggleLightSrv)
lights_on('high')
# TODO load it from configuration
# task 1
rospy.Subscriber('/qual_1_score', Qual1ScoringMsg, callback_topic, '/qual_1_score')
rospy.Subscriber('/scout_1/volatile_sensor', VolSensorMsg, callback_topic, '/scout_1/volatile_sensor')
rospy.Subscriber('/scout_1/camera/left/image_raw/compressed', CompressedImage, callback_topic,
'/scout_1/camera/left/image_raw/compressed')
rospy.Subscriber('/scout_1/camera/right/image_raw/compressed', CompressedImage, callback_topic,
'/scout_1/camera/right/image_raw/compressed')
steering_msg = Float64()
steering_msg.data = 0
effort_msg = Float64()
effort_msg.data = 0
QSIZE = 10
vel_fl_publisher = rospy.Publisher('/scout_1/fl_wheel_controller/command', Float64, queue_size=QSIZE)
vel_fr_publisher = rospy.Publisher('/scout_1/fr_wheel_controller/command', Float64, queue_size=QSIZE)
vel_bl_publisher = rospy.Publisher('/scout_1/bl_wheel_controller/command', Float64, queue_size=QSIZE)
vel_br_publisher = rospy.Publisher('/scout_1/br_wheel_controller/command', Float64, queue_size=QSIZE)
steering_fl_publisher = rospy.Publisher('/scout_1/fl_steering_arm_controller/command', Float64, queue_size=QSIZE)
steering_fr_publisher = rospy.Publisher('/scout_1/fr_steering_arm_controller/command', Float64, queue_size=QSIZE)
steering_bl_publisher = rospy.Publisher('/scout_1/bl_steering_arm_controller/command', Float64, queue_size=QSIZE)
steering_br_publisher = rospy.Publisher('/scout_1/br_steering_arm_controller/command', Float64, queue_size=QSIZE)
r = rospy.Rate(100)
osgar_debug('starting ...')
while True:
try:
message = ""
try:
while 1:
message = g_socket2.recv(zmq.NOBLOCK)
except:
pass
message_type = message.split(" ")[0]
if message_type == "cmd_rover":
arr = [float(x) for x in message.split()[1:]]
for pub, angle in zip(
[steering_fl_publisher, steering_fr_publisher, steering_bl_publisher, steering_br_publisher],
arr[:4]):
steering_msg.data = angle
pub.publish(steering_msg)
for pub, effort in zip(
[vel_fl_publisher, vel_fr_publisher, vel_bl_publisher, vel_br_publisher],
arr[4:]):
effort_msg.data = effort
pub.publish(effort_msg)
elif message_type == "request_origin":
osgar_debug('calling request_origin')
print("Requesting true pose")
try:
rospy.wait_for_service("/scout_1/get_true_pose", timeout=2.0)
request_origin = rospy.ServiceProxy('/scout_1/get_true_pose', LocalizationSrv)
p = request_origin(True)
s = "origin scout_1 %f %f %f %f %f %f %f" % (p.pose.position.x, p.pose.position.y, p.pose.position.z,
p.pose.orientation.x, p.pose.orientation.y, p.pose.orientation.z, p.pose.orientation.w)
print(s)
socket_send(s)
except rospy.service.ServiceException as e:
print(e)
osgar_debug('rospy exception')
elif message_type == "artf":
osgar_debug('calling artf')
s = message.split()[1:] # ignore "artf" prefix
x, y, z = [float(a) for a in s[1:]]
pose = geometry_msgs.msg.Point(x, y, z)
vol_type = s[0]
print("Reporting artifact %s at position %f %f %f" % (vol_type, x, y, z))
try:
rospy.wait_for_service("/vol_detected_service", timeout=2.0)
report_artf = rospy.ServiceProxy('/vol_detected_service', Qual1ScoreSrv)
resp = report_artf(pose=pose, vol_type=vol_type)
print("Volatile report result: %r" % resp.result)
osgar_debug('volatile result:' + str(resp.result))
except rospy.ServiceException as exc:
print("/vol_detected_service exception: " + str(exc))
osgar_debug('volatile exception')
else:
if len(message_type) > 0:
print("Unhandled message type: %s" % message_type)
except zmq.error.Again:
pass
r.sleep()
def __init__(self, ui, ally=None, active=True, interval=250):
super(Ally, self).__init__()
if ally is None:
return
self.ally = ally
# Create a dict so we now where we currently are
self.current = {
'velocity': None,
'my_state': None,
'opponent_state': None,
'ball_state': None,
'pidinfo': None,
'desired_position': None,
'desired_position_safe': None,
'other_opponent_state': None,
'other_ally_state': None
}
# Create a dictionary for last messages of different types
self.last = copy.deepcopy(self.current)
# Click to Drive positions (CTD)
self.CTD_x1 = self.CTD_y1 = None
self.CTD_x2 = self.CTD_y2 = None
# Setup the UI for this ally
self.ui = AllyUI(ui, ally=ally)
# After we've set up the GUI, if this ally is not active just bail
if not active:
self.ui.append_title('Inactive')
return
# Placeholders for child windows
self._step_resp = None
# Figure out my namespace based on who I am
ns = '/ally{}'.format(ally)
self.ns = ns
# If I am this ally, who is the other ally?
other_ally = 1 if ally == 2 else 2
# Connect ROS things
rospy.Subscriber('{}/ally{}_state'.format(ns,ally), \
RobotState, self._handle_my_state)
rospy.Subscriber('{}/ally{}_state'.format(ns,other_ally), \
RobotState, self._handle_other_ally_state)
rospy.Subscriber('{}/opponent{}_state'.format(ns,ally), \
RobotState, self._handle_opponent_state)
rospy.Subscriber('{}/opponent{}_state'.format(ns,other_ally), \
RobotState, self._handle_other_opponent_state)
rospy.Subscriber('{}/ball_state'.format(ns), \
BallState, self._handle_ball_state)
rospy.Subscriber('{}/desired_position'.format(ns), \
Pose2D, self._handle_des_pos)
rospy.Subscriber('{}/desired_position_safe'.format(ns), \
Pose2D, self._handle_des_pos_safe)
rospy.Subscriber('{}/vel_cmds'.format(ns), \
Twist, self._handle_vel)
rospy.Subscriber('{}/pidinfo'.format(ns), \
PIDInfo, self._handle_PID_error)
self.pub_des_pos = rospy.Publisher('{}/desired_position'.format(ns), \
Pose2D, queue_size=10)
# Connect Qt Buttons
self.ui.btn_clear.clicked.connect(self._btn_clear)
self.ui.btn_kick.clicked.connect(self._btn_kick)
self.ui.btn_battery.clicked.connect(self._btn_battery)
self.ui.btn_set_des_pos.clicked.connect(self._btn_des_pos)
self.ui.btn_stop_moving.clicked.connect(self._btn_stop_moving)
self.ui.btn_step_resp.clicked.connect(self._btn_step_resp)
# Connect Plot Mouse events
self.ui.plot_field.canvas.mpl_connect('button_press_event', self._plot_field_mouse_down)
self.ui.plot_field.canvas.mpl_connect('motion_notify_event', self._plot_field_mouse_move)
self.ui.plot_field.canvas.mpl_connect('button_release_event', self._plot_field_mouse_up)
# matplotlib create animation
# Look into using the same event_source for the two -- maybe this would help
# decrease overhead/latency? See matplotlib source code for more info
## Interval - I had interval=8ms and everything was smooth, but CPU was
## at 130%. I changed it to 500ms and it went down to 30%! But the plotting
## was severly discretized. A good in between was at 250ms (~60% CPU), but if
## you're on a faster machine, you may want to increase interval to ~100ms
self.animation_field = animation.FuncAnimation(self.ui.plot_field.canvas.fig, \
self._animate_field, init_func=self._animate_init_field, \
frames=None, interval=interval, blit=False, event_source=None)
import rospy
from nav_msgs.msg import Odometry
rospy.init_node('Odom_Subscriber')
odom = Odometry()
def callback(msg):
print msg.pose.pose.position.x
print msg.pose.pose.position.y
print msg.pose.pose.position.z
print '------------'
sub = rospy.Subscriber('/odom', Odometry, callback)
rospy.spin()
def __init__(self):
self.input_topic = rospy.get_param('~input_topic', '/cmd_vel')
self.override_topic = rospy.get_param('~override_topic', '/override')
self.analog_out_UID = rospy.get_param('~tinkerforge_analog_out_UID',
'Boq')
self.relay_UID = rospy.get_param('~tinkerforge_relay_UID', 'FMY')
self.stepper_left_UID = rospy.get_param(
'~tinkerforge_stepper_left_UID', '6QEPuu')
self.stepper_right_UID = rospy.get_param(
'~tinkerforge_stepper_right_UID', '6rnvVj')
self.analog_in_UID = rospy.get_param('~tinkerforge_analog_in_UID',
'F52')
self.tinkerforge_host = rospy.get_param('~tinkerforge_host',
'localhost')
self.tinkerforge_port = rospy.get_param('~tinkerforge_port', 4223)
self.max_speed_millivolt = rospy.get_param('~max_speed_millivolt', 300)
self.controller_p = rospy.get_param('~controller_p', 500.0)
self.deadzone = rospy.get_param('~deadzone', 0.10)
self.actual_steering = 0
self.sub = rospy.Subscriber(self.input_topic,
Twist,
self.cmd_vel_callback,
queue_size=10)
self.sub_override = rospy.Subscriber(self.override_topic,
Bool,
self.override_callback,
queue_size=10)
self.override = False
self.ipcon = IPConnection() # Create IP connection
self.ao = BrickletAnalogOutV2(self.analog_out_UID, self.ipcon)
self.idr = BrickletIndustrialDualRelay(self.relay_UID, self.ipcon)
self.ai = BrickletAnalogInV3(self.analog_in_UID, self.ipcon)
self.stepper_left = BrickSilentStepper(self.stepper_left_UID,
self.ipcon)
self.stepper_right = BrickSilentStepper(self.stepper_right_UID,
self.ipcon)
self.ai.register_callback(self.ai.CALLBACK_VOLTAGE,
self.steering_callback)
self.ipcon.connect(self.tinkerforge_host, self.tinkerforge_port)
self.ai.set_voltage_callback_configuration(50, False, "x", 0, 0)
self.ao.set_output_voltage(0)
self.idr.set_value(False, False)
self.actual_steering = 0
self.steering_setpoint = 0
self.stepper_motor_current = 800
self.stepper_max_velocity = 2000
self.stepper_speed_ramping_accel = 0
self.stepper_speed_ramping_deaccel = 0
self.stepper_left.set_motor_current(
self.stepper_motor_current) # 800mA
self.stepper_left.set_step_configuration(
self.stepper_left.STEP_RESOLUTION_8,
True) # 1/8 steps (interpolated)
self.stepper_left.set_max_velocity(self.stepper_max_velocity)
self.stepper_left.set_speed_ramping(self.stepper_speed_ramping_accel,
self.stepper_speed_ramping_deaccel)
self.stepper_left.enable()
self.stepper_right.set_motor_current(
self.stepper_motor_current) # 800mA
self.stepper_right.set_step_configuration(
self.stepper_right.STEP_RESOLUTION_8,
True) # 1/8 steps (interpolated)
self.stepper_right.set_max_velocity(self.stepper_max_velocity)
self.stepper_right.set_speed_ramping(
self.stepper_speed_ramping_accel,
self.stepper_speed_ramping_deaccel)
self.stepper_right.enable()
self.controller_timer = rospy.Timer(
period=rospy.Duration(1 / 200.0),
callback=self.controller_timer_callback)
def startNode():
c = Faster_Commands()
s = rospy.Service("/change_mode",MissionModeChange,c.srvCB)
rospy.Subscriber("state", State, c.stateCB)
rospy.spin()
##############################################################################################
# Main
##############################################################################################
pos_x, pos_y= 0, 0
awind,psi = 0, 0
theta = 0
lat_lon_origin = [[0,0],[0,0]]
zone_to_stay = [0,0]
u = [0,0]
if __name__ == '__main__':
node_name = 'controler_station_keeping'
rospy.init_node(node_name)
rospy.Subscriber("simu_send_theta", Vector3, sub_theta)
#rospy.Subscriber("simu_send_xy", Point, sub_xy)
rospy.Subscriber("simu_send_wind_direction", Float32, sub_wind_direction)
rospy.Subscriber("simu_send_wind_force", Float32, sub_wind_force)
rospy.Subscriber("launch_send_gps_origin", Vector3, sub_gps_origin)
rospy.Subscriber("simu_send_gps", GPSFix, sub_gps)
rospy.Subscriber("control_send_zone_to_stay", Vector3, sub_zone_to_stay)
pub_send_u_rudder = rospy.Publisher('control_send_u_rudder', Float32, queue_size=10)
pub_send_u_sail = rospy.Publisher('control_send_u_sail', Float32, queue_size=10)
u_rudder_msg = Float32()
u_sail_msg = Float32()
while lat_lon_origin == [[],[]] and not rospy.is_shutdown():
rospy.sleep(0.5)
rospy.loginfo("[{}] Waiting GPS origin".format(node_name))
def subscribe_infrared(self):
rospy.Subscriber(INFRARED_TOPIC, Infrared, self.__callback)
rospy.spin()
#!/usr/bin/python
import roslib
import rospy
import tf
def handle_rockie_pose(msg):
pass
if __name__ == '__main__':
rospy.init_node('spoofed_transforms')
rospy.Subscriber('')
def __init__(self):
print "Initializing Launchpad Class"
#######################################################################################################################
#Sensor variables
self._Counter = 0
self._left_encoder_value = 0
self._right_encoder_value = 0
self._battery_value = 0
self._ultrasonic_value = 0
self._qx = 0
self._qy = 0
self._qz = 0
self._qw = 0
self._left_wheel_speed_ = 0
self._right_wheel_speed_ = 0
self._LastUpdate_Microsec = 0
self._Second_Since_Last_Update = 0
self.robot_heading = 0
#######################################################################################################################
#Get serial port and baud rate of Tiva C Launchpad
port = rospy.get_param("~port", "/dev/ttyACM0")
baudRate = int(rospy.get_param("~baudRate", 115200))
#######################################################################################################################
rospy.loginfo("Starting with serial port: " + port + ", baud rate: " + str(baudRate))
#Initializing SerialDataGateway with port, baudrate and callback function to handle serial data
self._SerialDataGateway = SerialDataGateway(port, baudRate, self._HandleReceivedLine)
rospy.loginfo("Started serial communication")
#######################################################################################################################
#Subscribers and Publishers
#Publisher for left and right wheel encoder values
self._Left_Encoder = rospy.Publisher('lwheel',Int64,queue_size = 10)
self._Right_Encoder = rospy.Publisher('rwheel',Int64,queue_size = 10)
#Publisher for Battery level(for upgrade purpose)
self._Battery_Level = rospy.Publisher('battery_level',Float32,queue_size = 10)
#Publisher for Ultrasonic distance sensor
self._Ultrasonic_Value = rospy.Publisher('ultrasonic_distance',Float32,queue_size = 10)
#Publisher for IMU rotation quaternion values
self._qx_ = rospy.Publisher('qx',Float32,queue_size = 10)
self._qy_ = rospy.Publisher('qy',Float32,queue_size = 10)
self._qz_ = rospy.Publisher('qz',Float32,queue_size = 10)
self._qw_ = rospy.Publisher('qw',Float32,queue_size = 10)
#Publisher for entire serial data
self._SerialPublisher = rospy.Publisher('serial', String,queue_size=10)
#######################################################################################################################
#Subscribers and Publishers of IMU data topic
self.frame_id = '/base_link'
self.cal_offset = 0.0
self.orientation = 0.0
self.cal_buffer =[]
self.cal_buffer_length = 1000
self.imu_data = Imu(header=rospy.Header(frame_id="base_link"))
self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
self.imu_data.linear_acceleration_covariance = [-1,0,0,0,0,0,0,0,0]
self.gyro_measurement_range = 150.0
self.gyro_scale_correction = 1.35
self.imu_pub = rospy.Publisher('imu/data', Imu,queue_size = 10)
self.deltat = 0
self.lastUpdate = 0
#New addon for computing quaternion
self.pi = 3.14159
self.GyroMeasError = float(self.pi * ( 40 / 180 ))
self.beta = float(math.sqrt(3 / 4) * self.GyroMeasError)
self.GyroMeasDrift = float(self.pi * ( 2 / 180 ))
self.zeta = float(math.sqrt(3 / 4) * self.GyroMeasDrift)
self.beta = math.sqrt(3 / 4) * self.GyroMeasError
self.q = [1,0,0,0]
#######################################################################################################################
#Speed subscriber
self._left_motor_speed = rospy.Subscriber('left_wheel_speed',Float32,self._Update_Left_Speed)
self._right_motor_speed = rospy.Subscriber('right_wheel_speed',Float32,self._Update_Right_Speed)
global x
global y
global z
x = dado.pose.pose.position.x
y = dado.pose.pose.position.y
z = dado.pose.pose.position.z
if __name__=="__main__":
rospy.init_node("print_odom")
# Cria um subscriber que chama recebeu_leitura sempre que houver nova odometria
recebe_scan = rospy.Subscriber(topico_odom, Odometry , recebeu_leitura)
vel_pub = rospy.Publisher("bebop/cmd_vel", Twist, queue_size = 1)
takeoff = rospy.Publisher('bebop/takeoff', Empty, queue_size = 1, latch=True)
landing = rospy.Publisher('bebop/land', Empty, queue_size = 1, latch=True)
zerov = Twist(Vector3(0,0,0), Vector3(0,0,0))
v = 0.3 # Velocidade linear
vel = Twist(Vector3(v,0,0), Vector3(0,0,0))
count = 15
x0 = -1000
y0 = -1000
try:
transitions={
'outcome1': 'Go_circle',
'outcome2': 'outcome4'
})
smach.StateMachine.add('Go_circle',
Go_circle(),
transitions={'outcome1': 'Go_Straight'})
sis = smach_ros.IntrospectionServer('server_name', sm, '/LAB10_ROS_SMACH')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def cbPose(msg):
global x, y, yaw
x = msg.data[0]
y = msg.data[1]
yaw = msg.data[2]
# print x,y,yaw
if __name__ == '__main__':
sub_odom = rospy.Subscriber("/odometry", Float64MultiArray, cbPose)
main()
def __init__(self):
# speed (rpm) = self.SPEED_TO_ERPM_OFFSET + self.SPEED_TO_ERPM_GAIN * speed (m/s)
self.SPEED_TO_ERPM_OFFSET = float(
rospy.get_param("vesc/speed_to_erpm_offset", 0.0))
self.SPEED_TO_ERPM_GAIN = float(
rospy.get_param("vesc/speed_to_erpm_gain", 4614.0))
# servo angle = self.STEERING_TO_SERVO_OFFSET + self.STEERING_TO_SERVO_GAIN * steering_angle (rad)
self.STEERING_TO_SERVO_OFFSET = float(
rospy.get_param("vesc/steering_angle_to_servo_offset", 0.5304))
self.STEERING_TO_SERVO_GAIN = float(
rospy.get_param("vesc/steering_angle_to_servo_gain", -1.2135))
# Length of the car
self.CAR_LENGTH = float(rospy.get_param("vesc/chassis_length", 0.33))
# Width of the car
self.CAR_WIDTH = float(rospy.get_param("vesc/wheelbase", 0.25))
# The radius of the car wheel in meters
self.CAR_WHEEL_RADIUS = 0.0976 / 2.0
# Rate at which to publish joints and tf
self.UPDATE_RATE = float(rospy.get_param("~update_rate", 20.0))
# Speed noise mean is computed as the most recent speed multiplied by this value
self.SPEED_OFFSET = float(rospy.get_param("~speed_offset", 0.00))
# Speed noise std dev
self.SPEED_NOISE = float(rospy.get_param("~speed_noise", 0.0001))
# Steering angle noise mean is cimputed as the most recent steering angle multiplied by this value
self.STEERING_ANGLE_OFFSET = float(
rospy.get_param("~steering_angle_offset", 0.00))
# Steering angle noise std dev
self.STEERING_ANGLE_NOISE = float(
rospy.get_param("~steering_angle_noise", 0.000001))
# Forward direction noise mean
self.FORWARD_OFFSET = float(rospy.get_param("~forward_offset", 0.0))
# Forward direction noise std dev
self.FORWARD_FIX_NOISE = float(
rospy.get_param("~forward_fix_noise", 0.0000001))
# Additional zero-mean gaussian noise added to forward direction
# std dev is most recent velocity times this value
self.FORWARD_SCALE_NOISE = float(
rospy.get_param("~forward_scale_noise", 0.001))
# Side direction noise mean
self.SIDE_OFFSET = float(rospy.get_param("~side_offset", 0.0))
# Side direction noise std dev
self.SIDE_FIX_NOISE = float(
rospy.get_param("~side_fix_noise", 0.000001))
# Additional zero-mean gaussian noise added to side direction
# std dev is most recent velocity times this value
self.SIDE_SCALE_NOISE = float(
rospy.get_param("~side_scale_noise", 0.001))
# Theta noise mean
self.THETA_OFFSET = float(rospy.get_param("~theta_offset", 0.0))
# Theta noise std dev
self.THETA_FIX_NOISE = float(
rospy.get_param("~theta_fix_noise", 0.000001))
# Forces the base_footprint tf to stay in bounds, i.e. updates to the odometry
# that make base_footprint go out of bounds will be ignored.
# Only set to true if a map is available.
self.FORCE_IN_BOUNDS = bool(rospy.get_param("~force_in_bounds", False))
# Disable publishing car_pose if true.
self.USE_MOCAP = bool(rospy.get_param("~use_mocap", False))
# Append this prefix to any broadcasted TFs
self.TF_PREFIX = str(rospy.get_param("~tf_prefix", "").rstrip("/"))
if len(self.TF_PREFIX) > 0:
self.TF_PREFIX = self.TF_PREFIX + "/"
# The map and map params
self.permissible_region = None
self.map_info = None
# Get the map
if self.FORCE_IN_BOUNDS:
self.permissible_region, self.map_info = self.get_map()
# The most recent time stamp
self.last_stamp = None
# The most recent speed (m/s)
self.last_speed = 0.0
self.last_speed_lock = Lock()
# The most recent steering angle (rad)
self.last_steering_angle = 0.0
self.last_steering_angle_lock = Lock()
# The most recent transform from odom to base_footprint
self.cur_odom_to_base_trans = np.array([0, 0], dtype=np.float)
self.cur_odom_to_base_rot = 0.0
self.cur_odom_to_base_lock = Lock()
# The most recent transform from the map to odom
self.cur_map_to_odom_trans = np.array([0, 0], dtype=np.float)
self.cur_map_to_odom_rot = 0.0
self.cur_map_to_odom_lock = Lock()
# MOCAP only
self.car_pose = None
# Message used to publish joint values
self.joint_msg = JointState()
self.joint_msg.name = [
"front_left_wheel_throttle",
"front_right_wheel_throttle",
"back_left_wheel_throttle",
"back_right_wheel_throttle",
"front_left_wheel_steer",
"front_right_wheel_steer",
]
self.joint_msg.position = [0, 0, 0, 0, 0, 0]
self.joint_msg.velocity = []
self.joint_msg.effort = []
# Publishes joint messages
self.br = tf.TransformBroadcaster()
# Duration param controls how often to publish default map to odom tf
# if no other nodes are publishing it
self.transformer = tf.TransformListener()
# Publishes joint values
if not self.USE_MOCAP:
self.state_pub = rospy.Publisher("car_pose",
PoseStamped,
queue_size=1)
# Publishes joint values
self.cur_joints_pub = rospy.Publisher("joint_states",
JointState,
queue_size=1)
# Subscribes to the initial pose of the car
self.init_pose_sub = rospy.Subscriber("initialpose",
PoseWithCovarianceStamped,
self.init_pose_cb,
queue_size=1)
# Subscribes to info about the bldc (particularly the speed in rpm)
self.speed_sub = rospy.Subscriber("vesc/sensors/core",
VescStateStamped,
self.speed_cb,
queue_size=1)
# Subscribes to the position of the servo arm
self.servo_sub = rospy.Subscriber(
"vesc/sensors/servo_position_command",
Float64,
self.servo_cb,
queue_size=1)
# Subscribes to the pose from mocap
if self.USE_MOCAP:
self.pose_sub = rospy.Subscriber("car_pose",
PoseStamped,
self.pose_cb,
queue_size=1)
# Timer to updates joints and tf
self.update_timer = rospy.Timer(
rospy.Duration.from_sec(1.0 / self.UPDATE_RATE), self.timer_cb)
rospy.wait_for_service('pick_place_routine')
# Output request parameters into output yaml file
yaml_filename = 'output_' + str(test_scene_num.data) + '.yaml'
send_to_yaml(yaml_filename, dict_list)
print(yaml_filename + " has been saved.")
if __name__ == '__main__':
# ROS node initialization
rospy.init_node('Perception', anonymous=True)
# Create Subscribers
pcl_sub = rospy.Subscriber("/pr2/world/points", pc2.PointCloud2, pcl_callback, queue_size=1)
# Create Publishers
pcl_objects_pub = rospy.Publisher("/pcl_objects", PointCloud2, queue_size=1)
pcl_table_pub = rospy.Publisher("/pcl_table", PointCloud2, queue_size=1)
pcl_cluster_pub = rospy.Publisher("/pcl_cluster", PointCloud2, queue_size=1)
object_markers_pub = rospy.Publisher("/object_markers", Marker, queue_size=1)
detected_objects_pub = rospy.Publisher("/detected_objects", DetectedObjectsArray, queue_size=1)
# Load Model From disk
model = pickle.load(open('model.sav', 'rb'))
clf = model['classifier']
encoder = LabelEncoder()
encoder.classes_ = model['classes']
scaler = model['scaler']
def __init__(self):
'''--------------Adjust before running program-----------------'''
# increment if you wish to save a new version of the network model
# or set to specific model version if you wish to use an existing
# model
self.path_nr = 5
# set to False if you wish to run program with existing model
self.learn = True
'''---------------------Hyperparameters------------------------'''
# hyperparameters to experiment with
# number of learning episodes
self.max_episodes = 475
self.max_steps_per_episode = 400
# speed of the robot's wheels
self.speed = 15.0
# replay buffer capacity
self.rb_capacity = 10000
# number of examples that will be extracted at once from
# the memory
self.batch_size = 100
# number of memory samples that will be processed together in
# one execution of the neural network
self.mini_batch_size = 1
# variables for Bellman equation
self.gamma = 0.95
self.alpha = 0.95
# update rate for target network
self.update_r_targets = 5
# integer variable after how many episodes exploiting is possible
self.start_decaying = (self.max_episodes / 5)
'''------------------------Class objects-----------------------'''
# helper classes
self.bot = bt.Bot() # reward + q-matrix
self.imgHelper = mi.MyImage() # image processing
self.memory = Memory.Memory(self.rb_capacity) # replay buffer
'''--------------------------Images----------------------------'''
# current image
self.my_img = np.zeros(50)
# image stack
self.image_stack = np.zeros(shape=[self.mini_batch_size, 50])
# flattened current multiple images
self.my_mult_img = np.zeros(shape=[1, self.mini_batch_size * 50])
# last images
self.last_imgs = np.copy(self.my_mult_img)
# number of images received in total
self.img_cnt = 0
'''-------------------------Episodes---------------------------'''
# episodes
self.explorationMode = False # exploring or exploiting?
self.episode_counter = 0 # number of done episodes
self.step_counter = 0 # counts every while iteration
self.steps_in_episode = 0 # counts steps inside current episode
# variables deciding whether to explore or to exploit
# old --> these are currently used
self.exploration_prob = 0.99
# self.exploration_prob = 0.8
self.decay_rate = 0.01
self.min_exploration_rate = 0.01
self.max_exploration_rate = 1
self.decay_per_episode = self.calc_decay_per_episode()
# new
self.epsilon = 1
self.epsilon_min = 0.001
self.epsilon_decay = 0.999
'''-------------------------Strings----------------------------'''
# strings to display actions
self.action_strings = {
0: "sharp left",
1: "left",
2: "slightly left",
3: "forward",
4: "slightly right",
5: "right",
6: "sharp right",
7: "stop"
}
# strings to display states
self.state_strings = {
0: "far left",
1: "left",
2: "slightly left",
3: "middle",
4: "slightly right",
5: "right",
6: "far right",
7: "lost"
}
'''----------------------Neural network------------------------'''
# neural network
# tensorflow session object
self.sess = tf.compat.v1.Session()
# policy network
input_shape = np.shape(self.my_mult_img)
self.policy_net = Network.Network(mini_batch_size=self.mini_batch_size)
# target array (for simple way)
self.targets = np.zeros(shape=[1, (len(self.action_strings) - 1)])
# target network to calculate 'optimal' q-values
self.target_net = Network.Network(mini_batch_size=self.mini_batch_size)
# copy weights and layers from the policy net into the target net
self.target_net = self.policy_net.copy(self.target_net)
'''--------------------------Driving---------------------------'''
# terminal states
self.lost_line = 7
self.stop_action = 7
# current state and action (initially negative)
self.curr_state = -1
self.curr_action = -1
self.last_state = self.curr_state
# starting coordinates of the robot
self.x_position = -0.9032014349
self.y_position = -6.22487658223
self.z_position = -0.0298790967155
# deviation from speed to turn the robot to the left or right
# sharp curve => big difference
self.sharp = self.speed * (1.0 / 7.0)
# middle curve => middle difference
self.middle = self.speed * (1.0 / 8.5)
# slight curve => slight difference
self.slightly = self.speed * (1.0 / 10.0)
# flag to indicate end of learning
self.first_time = True
'''----------------------------ROS-----------------------------'''
# message to post on topic /cmd_vel
self.vel_msg = Twist()
# ROS variables
# publisher to publish on topic /cmd_vel
self.velocity_publisher = rospy.Publisher('/cmd_vel',
Twist,
queue_size=100)
# initializing ROS-node
rospy.init_node('reinf_matrix_driving', anonymous=True)
# subscriber for topic '/camera/image_raw'
self.sub = rospy.Subscriber('/camera/image_raw', Image,
self.cam_im_raw_callback)
'''------------------------statistics--------------------------'''
self.step_array = np.zeros(shape=[self.max_episodes + 1])
# FSM state: after stopped go back to half-sitting
def waitHS(self, rs):
if self.hsCB is not None and self.hsCB.check():
self.hsCB = None
goHalfSitting(qpsolver, postureTask1, hrp4_q, \
[dynamicsConstraint1, contactConstraint], \
[kinConstraint1])
self.fsm = self.idle
def idle(self, rs):
# robotH = hrp4
# bodyIdxR = robotH.bodyIndexByName('r_ankle')
# posR=(list(robotH.mbc.bodyPosW)[bodyIdxR]).translation()
# print posR[0]
#
# bodyIdxL = robotH.bodyIndexByName('l_ankle')
# posL=(list(robotH.mbc.bodyPosW)[bodyIdxL]).translation()
# print posL[0]
pass
ask_user.askUser('start', 'Start')
controller = Controller()
rospy.Subscriber('/robot/sensors/robot_state',
MCRobotState,
controller.run,
queue_size=10,
tcp_nodelay=True)
rospy.spin()
if(control_effort < 0):
esc_cmd.position = .305
elif(control_effort >= 60):
esc_cmd.position = .345
else:
esc_cmd.position = .305 + control_effort/1200.0
esc_cmd.joint_name = 'ESC'
pub_MotorCmd.publish(esc_cmd)
'''comment me out to return steering controll'''
servo_cmd = MotorCommand()
servo_cmd.joint_name = 'Servo'
servo_cmd.position = 0.0
pub_MotorCmd.publish(servo_cmd)
if __name__ == '__main__':
print "hello I am the esc and servo. for now"
rospy.init_node("motor_control_pub", anonymous = False)
pub_MotorCmd = rospy.Publisher("/pololu/command", MotorCommand, queue_size = 1)
rospy.Subscriber("/motor_control_effort", Float64, callback)
rospy.spin()
def listener():
"""
Subscribes to topic "exo_info".
:return: -
"""
def map_exo(data):
"""
Teleoperatates the Baxter robot.
:param data: exo_info message
:return: -
"""
global r_last_command_baxter_angles
global l_last_command_baxter_angles
global mode
global max_lin
global max_ang
global des_baxter_state
global act_baxter_state
[des_baxter_state,act_baxter_state]=get_state(data,right,left,des_baxter_state,act_baxter_state)
r_end_point=data.end_point[0:3]
l_end_point=data.end_point[3:6]
r_rpy=data.rpy[0:3]
l_rpy=data.rpy[3:6]
l_z_button=data.buttons[0]
l_c_button=data.buttons[1]
r_z_button=data.buttons[2]
r_c_button=data.buttons[3]
r_ax=data.analog[0]/128.0
r_ay=data.analog[1]/128.0
l_ax=data.analog[2]/128.0
l_ay=data.analog[3]/128.0
imu=data.imu*1.0
imu=threshold(imu,1)
##right arm
#moving a manipulator if c button of the right Nunchuk is being pressed
if r_c_button==1:
if mode==0:
set_j(right, rj, des_baxter_state[0,0:7])
r_last_command_baxter_angles[:]=des_baxter_state[0,0:7]
elif (mode==1 or mode==2) :
set_pose(right,kin_r,rj,r_end_point,r_rpy,des_baxter_state[0,2],0,mode)
else :
set_j(right, rj, r_last_command_baxter_angles)
#Closing the right gripper if z button of the right Nunchuk is being pressed
if r_z_button==1:
grip_right.close()
else:
grip_right.open()
##left arm
#moving a manipulator if c button of the left Nunchuk is being pressed
if l_c_button==1:
if mode==0:
set_j(left, lj, des_baxter_state[0,7:14])
l_last_command_baxter_angles[:]=des_baxter_state[0,7:14]
elif (mode==1 or mode==2) :
set_pose(left,kin_l,lj,l_end_point,l_rpy,des_baxter_state[0,9],1,mode)
else :
set_j(left, lj, l_last_command_baxter_angles)
#Closing the left gripper if z button of the right Nunchuk is being pressed
if l_z_button==1:
grip_left.close()
else:
grip_left.open()
#move both arms in catesian frame by the left joy stick of the Nunchuk
if l_ay!=0 or l_ax!=0:
move_catesian(right,left,kin_r,kin_l,rj,lj,0,-l_ax*0.01,l_ay*0.01)
#drive a mobile base by a right joy stick of the Nunchuk + the yaw rotation can also be controlled by the imu sensor of the exoskeleton
r_ax=r_ax*max_ang*10
r_ay=r_ay*max_lin*10
imu=-threshold(imu,1)/10
r_ay=threshold(r_ay,max_lin*0.1)
r_ax=threshold(r_ax,max_ang*0.1)
print("-- Speed: %.2f Direction: %.2f " % (r_ay, imu+r_ax))
speed = r_ay
direction = imu+r_ax
control_sig = Int32MultiArray()
sig = Float32MultiArray()
twist=Twist()
twist.linear.x=speed
twist.linear.y=0
twist.linear.z=0
twist.angular.x=0
twist.angular.y=0
twist.angular.z=direction
control_sig.data = [speed, direction, 0]
sig.data = [float(speed), float(direction)]
########## Uncomment these 3 lines to enable the IMU control
#pub_data.publish(twist)
#pub_resq.publish(control_sig)
#pub_base.publish(sig)
print("Initializing node... ")
rospy.init_node("teleoperation")
pub_resq = rospy.Publisher('/resqbot/control_sig', Int32MultiArray, queue_size=1)
pub_base = rospy.Publisher('base_control_sig', Float32MultiArray, queue_size=1)
pub_data = rospy.Publisher('/robot/quickie/diff_drive/command', Twist, queue_size=1)
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state().enabled
left = baxter_interface.Limb('left')
right = baxter_interface.Limb('right')
left.set_command_timeout(0.1)
right.set_command_timeout(0.1)
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
grip_right = baxter_interface.Gripper('right', CHECK_VERSION)
grip_right.calibrate()
grip_left.calibrate()
lj = left.joint_names()
rj = right.joint_names()
print("Enabling robot... ")
rs.enable()
#right.move_to_neutral(timeout=5.0)
kin_r = baxter_kinematics('right')
kin_l = baxter_kinematics('left')
print '\n*** Baxter Description ***\n'
#kin.print_robot_description()
print '\n*** Baxter KDL Chain ***\n'
#kin.print_kdl_chain()
rospy.Subscriber("exo_info", exo_info, map_exo)
rospy.spin()
def __init__(self, wp=None):
self.pub_h_nav = rospy.Publisher('height_control',
HControl,
queue_size=100)
self.pub_r_nav = rospy.Publisher('rotation_control',
RControl,
queue_size=100)
self.pub_m_nav = rospy.Publisher('movement_control',
MControl,
queue_size=100)
rospy.Subscriber('rotation_control_status',
RControl,
self.r_status_callback,
queue_size=100)
rospy.Subscriber('movement_control_status',
MControl,
self.m_status_callback,
queue_size=100)
rospy.Subscriber('height_control_status',
HControl,
self.h_status_callback,
queue_size=100)
self.h_control = HControl()
self.r_control = RControl()
self.m_control = MControl()
# used for HControl (int state, float depth, int power) #######################################
self.hStates = {
'down': 0,
'staying': 1,
'up': 2,
'unlock': 4,
'lock': 5
}
self.hState = None # state
self.depth = None # depth (nonstop moving: -1, moving distance: x)
self.hPower = 100 # power
# used for RControl (int state, float rotation, int power) ####################################
self.rStates = {
'left': 0, # rotate left
'staying': 1,
'right': 2, # rotate right
'rotate_front_cam_dist': 3, # rotate with fcd
'keep_rotate_front_cam_dist': 4 # keeping rotating with fcd
}
self.rState = None # state
self.rotation = None # rotation (nonstop rotating: -1, rotate degree: x)
self.rPower = 90 # power
# used for MControl (int state, int mDirection, float power, float distance) #######
self.mStates = {
'off': 0,
'power': 1, # adjust with power
'distance': 2, # ajust with distance
'front_cam_center': 3, # centered with front camera
'bot_cam_center': 4, # centered with bottom camera
'motor_time': 5 # turn on motor with specific time
}
self.mState = None # state
self.directions = {
'none': 0,
'forward': 1,
'right': 2,
'backward': 3,
'left': 4
}
self.mDirection = None # mDirection
self.mPower = None # power (none: 0, motor power: x)
self.distance = None # distance (distance away from the object: x)
self.runningTime = None # runningTime (time for the motor to turn on)
#waypoint variables
if wp:
self.waypoint = wp
else:
self.waypoint = Waypoint()
self.is_running_waypoint_rotation = False
self.is_running_waypoint_movement = False
self.is_busy_waypoint = False
self.w_distance_m = 0
self.w_power_m = 140
self.r_power = 85
self.h_power = 100
self.m_power = 140
self.waypoint_state = 0
self.thread_w = None
self.exit_waypoints = False
#vars dealing with movement break time
self.waypoint_m_time = 0
# self.waypoint_m_time_max = 2.14
self.waypoint_m_time_max = 1.0
#vars dealing with height checking
self.depth_threshold = 0.3
self.depth_assignment = 0.5
self.current_waypoint_x = 0
self.current_waypoint_y = 0
self.depth_cap = 3.5
#var for saved heading
self.saved_heading = None
self.saved_heading_path1 = None
def __init__(self):
rospy.init_node("CamRcv", anonymous=True)
self.sub = rospy.Subscriber("/camera/image_raw", Image, self.callback)
self.rate = rospy.Rate(10)
self.bridge = CvBridge()
def autopilot_listener():
rospy.init_node('kontrol_uav',anonymous=True)
rospy.Subscriber('mavros/rc/in',RCIn,autopilot_rc_in_status)
def listener ():
rospy.Subscriber("/gazebo/model_states", ModelStates, callback, queue_size=1)
rospy.spin()
def __init__(self):
self.image_pub = rospy.Publisher("/eyes/right", Image)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera2/usb_cam2/image_raw", Image,
self.callback)
def publish_plus_subscribe():
print("Hello")
pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=10)
#<name of publisher> = rospy.Publisher(<topic name, within '' or "", msg type,buffer size)
rospy.init_node('node_name', anonymous=True)
rospy.Subscriber('/odom', Odometry, callback)
#rospy.init_node(<Name of Node > )
# This name is extremely important, as this name will be used by ROS to refer to this script.
#Required only once per script.
#anonymous=True is required for ensuring node name is unique.
freq = 100 #rate at which simulation is run
rate = rospy.Rate(freq)
x_des = 3 #desired coordinates
y_des = 3
tolerance = 0.1 #value to assess destination is reached
kp_lin = 0.08 #linear positional
kp_ang = 0.6 #angular positional
kd_lin = 0.1 #linear differential
kd_ang = 0.1 #angular differential
ki_lin = 0.01 #linear integral
ki_ang = 0.001 #angular integral
error_lin_prev = math.sqrt((x_des - x_curr)**2 + (y_des - y_curr)**2)
error_ang_prev = math.atan2(
(y_des - y_curr),
(x_des - x_curr)) - math.atan2(y_curr - y_prev, x_curr - x_prev)
sum_err_lin = 0
sum_error_ang = 0
random123 = Twist()
pub.publish(random123)
a123 = rospy.Time.now()
b = a123.secs
#initialize lists for graphs
time_list = []
c = []
error_lin_list = []
error_ang_list = []
x_list = []
y_list = []
while not rospy.is_shutdown(): #run this loop until shutdown (<tolerance)
# calculate errors (position and velocity)
error_lin = math.sqrt((x_des - x_curr)**2 + (y_des - y_curr)**2)
d_error_lin = (error_lin - error_lin_prev) * freq
error_lin_prev = error_lin
sum_err_lin = sum_err_lin + error_lin
vel = kp_lin * error_lin + kd_lin * d_error_lin + ki_lin * sum_err_lin
theta1 = math.atan2(2 * (q_w * q_z), (1 - 2 * q_z * q_z))
error_ang = math.atan2((y_des - y_curr), (x_des - x_curr)) - theta1
d_error_ang = (error_ang - error_ang_prev) * freq
error_ang_prev = error_ang
sum_error_ang = sum_error_ang + error_ang
turn = kp_ang * error_ang + kd_ang * d_error_ang + ki_ang * sum_error_ang
a = Twist()
a.angular.z = turn
if vel > 0: #linear velocity limiter
a.linear.x = min(vel, 0.3)
else:
a.linear.x = max(vel, -0.3)
rospy.loginfo(error_ang)
d = rospy.Time.now()
time_list.append(d.secs)
rospy.loginfo(d.secs)
if (len(time_list) < 2):
rospy.loginfo(time_list[0])
c.append(time_list[0])
else:
rospy.loginfo(d.secs - time_list[1])
c.append(d.secs - time_list[1])
#add data to lists for graphs
error_lin_list.append(error_lin)
error_ang_list.append(error_ang)
x_list.append(x_curr)
y_list.append(y_curr)
pub.publish(a)
#if the destination is reached, call shutdown and plot data
if error_lin < tolerance:
rospy.loginfo("Reached Destination")
rospy.signal_shutdown(myhook)
plt.figure(1)
plt.plot(c, error_lin_list)
plt.ylim(0, 50)
plt.show()
plt.figure(2)
plt.plot(c, error_ang_list)
plt.show()
plt.figure(3)
plt.plot(x_list, y_list)
plt.show()
rate.sleep()
def __init__(self):
#--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--
#Constructor
#--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--..--**--
# Transitions between the states. 'trigger' is a function you can call
# on the class object which triggers the transition to occur. The
# functions listed in 'before' are functions within the class that will
# run just after the transition takes place. Calling the function returns
# the transition's success or lack thereof.
transitions = [
# Self States - Occur when a state is running
{
'source': 'Off',
'dest': None,
'after': 'Off',
'trigger': 'run'
},
{
'source': 'Traverse',
'dest': None,
'after': 'Traverse',
'trigger': 'run'
},
{
'source': 'Search',
'dest': None,
'after': 'Search',
'trigger': 'run'
},
{
'source': 'Destroy',
'dest': None,
'after': 'Destroy',
'trigger': 'run'
},
{
'source': 'Panning',
'dest': None,
'after': 'Panning',
'trigger': 'run'
},
{
'source': 'Complete',
'dest': None,
'after': 'Complete',
'trigger': 'run'
},
# Changes states - Occur to change between different stages of the task.
{
'source': ['Off', 'Traverse', 'Search'],
'dest': 'Traverse',
'after': 'startTraverse',
'trigger': 'toTraverse'
},
{
'source': ['Traverse', 'Panning'],
'dest': 'Search',
'after': 'startSearch',
'trigger': 'toSearch'
},
{
'source': ['Traverse', 'Search', 'Panning'],
'dest': 'Destroy',
'after': 'startDestroy',
'trigger': 'toDestroy'
},
{
'source': 'Destroy',
'dest': 'Search',
'after': 'startSearch',
'trigger': 'toSearch'
},
{
'source': 'Destroy',
'dest': 'Panning',
'after': 'startPanning',
'trigger': 'toPanning'
},
{
'source': 'Destroy',
'dest': 'Complete',
'after': 'startComplete',
'trigger': 'toComplete'
},
{
'source': 'Complete',
'dest': 'Traverse',
'after': 'startTraverse',
'trigger': 'toTraverse'
},
{
'source':
['Traverse', 'Search', 'Destroy', 'Panning', 'Complete'],
'dest': 'Off',
'after': 'startOff',
'trigger': 'Reset'
},
]
# Initialize the state Machine
self.mach = Machine(model=self,
states=AutonomousStateMachine.states,
initial='Off',
transitions=transitions,
ignore_invalid_triggers=True)
# Ros Publishers and Subscribers
self.drive_pub = rospy.Publisher("/core_rover/driver/drive_cmd",
DriveCmd,
queue_size=10)
self.tdrive_sub = rospy.Subscriber("/core_rover/driver/drive_cmd2",
DriveCmd, self.tdriveCallback)
self.gps_sub = rospy.Subscriber("/nova_common/gps_data", NavSatFix,
self.gpsCallback)
self.rpy_sub = rospy.Subscriber("/nova_common/RPY", RPY,
self.rpyCallback)
self.tb_sub = rospy.Subscriber("/core_rover/navigation/tennis_stat",
String, self.tbCallback)
# Initialise the global state parameter
self.setAutonomousMode('Off')
#add a normalization
cNorm = mpl.colors.Normalize(vmin=0, vmax=100)
#init the mapping
scalarMap = mtpltcm.ScalarMappable(norm=cNorm, cmap=colormap)
# Draw Map
im = plt.imshow(mag_map)
plt.colorbar()
# Param
pre_location = [0.0, 0.0];
# Start ROS Node
rospy.init_node('mag_recoder')
listener = tf.TransformListener()
rospy.Subscriber("m_mag",MagneticField, mag_CB)
rospy.Service('save_mag_data', SetBool, handle_save_img)
while not rospy.is_shutdown():
try:
(trans,rot) = listener.lookupTransform('/map', '/gyro_link', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
# print(trans)
if(calc_p2p_dist(pre_location[0],pre_location[1],trans[0],trans[1])%mag_map_resolution < mag_map_resolution):
y = int(trans[0]/mag_map_resolution - map_origin[0]/mag_map_resolution)
x = int(trans[1]/mag_map_resolution - map_origin[1]/mag_map_resolution)
if x>=height or y>= width or x<0 or y<0:
continue
