def main(unused_argv):
rospy.init_node('raven_vive_teleop')
temp_flag = 0
pre_position = [0, 0, 0]
pre_pose = [1, 0, 0, 0]
temp = [1, 0, 0, 0]
# same loop rate as gym environment
rate = rospy.Rate(60.0)
env = Environment(assets_root, disp=True, hz=60)
task = tasks.names[task_name]()
task.mode = mode
env.set_task(task)
obs = env.reset()
info = None
#agent = teleop_agent(env)
dataset = Dataset(
os.path.join(dataset_root, f'ravens-bc-rollout-{time.time_ns()}'))
seed = 0
br = tf.TransformBroadcaster()
rci = RobotControlInterface(env)
episode = []
reward = 0
done = False
plt.ion()
im_color = obs['color'][0]
im_depth = obs['depth'][0]
img_draw = plt.imshow(im_depth, interpolation='nearest')
agent = random_action_agent(env)
f = open(model_save_rootdir + "rollout_log.txt", "w+")
episode_steps = 0
n_episode = 1
while not rospy.is_shutdown():
#p.stepSimulation()
#p.getCameraImage(480, 320)
keys = p.getKeyboardEvents()
if ord("r") in keys and keys[ord("r")] & p.KEY_WAS_RELEASED:
print("reset env")
episode = []
obs = env.reset()
action = agent.act()
#if action != None:
# print(action)
i = 30
while (i > 0):
obs, reward, done, info, aux = env.step_simple(action,
use_aux=True)
i -= 1
state = get_state_ground_truth(env)
print(state)
print(aux)
env.reset()
rate.sleep()
def __init__(self):
self.rate = rospy.Rate(rospy.get_param("~frequency", 20))
self.Ncars = rospy.get_param("/num_cars", 3)
self.car_id = rospy.get_param("~car_id", 0)
self.frame_name = rospy.get_param("/frame_name")
self.frame_id = self.frame_name[self.car_id]
self.id_dict = rospy.get_param("/id_dict", None)
self.connections = rospy.get_param("/connections", None)
self.own_connections = self.connections[str(self.car_id)]
self.Nconn = len(self.own_connections)
self.full_graph = dict_to_graph.convert(self.connections)
self.graph = dict_to_graph.prune(self.full_graph, self.car_id)
self.meas = CarMeasurement()
self.meas.header = Header()
self.meas.header.frame_id = self.frame_id
self.meas.car_id = self.car_id
self.uwb_ranges = self.init_uwb()
self.gps = [None] * self.Nconn
self.control = [None] * self.Nconn
self.lidar = [None] * self.Nconn
self.first_time = True
self.br = tf.TransformBroadcaster()
self.debug = MeasurementDebug()
self.debug.header.frame_id = self.frame_id
self.debug_pub = rospy.Publisher("meas_debug",
MeasurementDebug,
queue_size=1)
self.meas_pub = rospy.Publisher("measurements",
CarMeasurement,
queue_size=1)
self.gps_sub = []
self.control_sub = []
self.control_sub2 = []
self.lidar_sub = []
# in case sensor data is broadcast on global topics
self.uwb_sub = rospy.Subscriber("/ranges",
UWBRange,
self.range_cb,
queue_size=1)
self.uwb_sub2 = []
# control topic is gathered currently for face cars but not real
self.control_sub.append(
rospy.Subscriber("/control",
CarControl,
self.control_cb,
queue_size=1))
self.lidar_sub.append(
rospy.Subscriber("/lidar_pose",
LidarPose,
self.lidar_cb,
queue_size=1))
# for sensor data broadcast in car namespaces
for i, ID in enumerate(self.own_connections):
self.gps_sub.append(
rospy.Subscriber("odom" + str(ID),
SimplePose,
self.gps_cb, (i, ),
queue_size=1))
self.gps_sub.append(
rospy.Subscriber("/car" + str(ID) + "/odom",
Odometry,
self.odom_cb, (ID, i),
queue_size=1))
self.control_sub.append(
rospy.Subscriber("/car" + str(ID) + "/control",
CarControl,
self.control_cb,
queue_size=1))
self.lidar_sub.append(
rospy.Subscriber("/car" + str(ID) + "/lidar_pose",
LidarPose,
self.lidar_cb,
queue_size=1))
self.lidar_sub.append(
rospy.Subscriber("/car" + str(ID) + "/poseupdate",
PoseWithCovarianceStamped,
self.slam_cb, (ID, ),
queue_size=1))
self.uwb_sub2.append(
rospy.Subscriber("/car" + str(ID) + "/ranges",
UWBRange,
self.range_cb,
queue_size=1))
def __init__(self, pozyx, ranging_protocol, robot_list, tag_pos,
robot_number, alpha, noise, R, link_to_robot, do_ranging,
tf_prefix):
self.pozyx = pozyx
self.ranging_protocol = ranging_protocol
self.tag_pos = tag_pos
self.robot_number = robot_number
self.link_to_robot = link_to_robot
self.do_ranging = do_ranging
self.tf_prefix = tf_prefix
self.distance_1 = pzx.DeviceRange()
self.distance_2 = pzx.DeviceRange()
self.distance_3 = pzx.DeviceRange()
self.distance_4 = pzx.DeviceRange()
self.A = robot_list[robot_number]['left']
self.B = robot_list[robot_number]['right']
if robot_number == 1:
self.C = robot_list[2]['left']
self.D = robot_list[2]['right']
elif robot_number == 2:
self.C = robot_list[1]['left']
self.D = robot_list[1]['right']
self.distance_prev_1 = 0
self.distance_prev_2 = 0
self.distance_prev_3 = 0
self.distance_prev_4 = 0
self.f1 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f1.P = 1
self.f1.H = np.array([[1.]])
self.f1.F = np.array([[1.]])
self.f1.B = np.array([[1.]])
self.f1.Q = noise
self.f1.R = R
self.f1.alpha = alpha
self.f2 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f2.P = 1
self.f2.H = np.array([[1.]])
self.f2.F = np.array([[1.]])
self.f2.B = np.array([[1.]])
self.f2.Q = noise
self.f2.R = R
self.f2.alpha = alpha
self.f3 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f3.P = 1
self.f3.H = np.array([[1.]])
self.f3.F = np.array([[1.]])
self.f3.B = np.array([[1.]])
self.f3.Q = noise
self.f3.R = R
self.f3.alpha = alpha
self.f4 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f4.P = 1
self.f4.H = np.array([[1.]])
self.f4.F = np.array([[1.]])
self.f4.B = np.array([[1.]])
self.f4.Q = noise
self.f4.R = R
self.f4.alpha = alpha
self.f5 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f5.P = 1
self.f5.H = np.array([[1.]])
self.f5.F = np.array([[1.]])
self.f5.B = np.array([[1.]])
self.f5.Q = 0.001
self.f5.R = 0.3
self.f5.alpha = 1
self.pozyx.setRangingProtocol(self.ranging_protocol)
self.br = tf.TransformBroadcaster()
#!/usr/bin/env python
import roslib
roslib.load_manifest('subt_rviz_plugins')
from sensor_msgs.msg import LaserScan
import rospy
from math import cos, sin
import tf
topic = 'scan'
publisher = rospy.Publisher(topic, LaserScan, queue_size=5)
rospy.init_node('test_scan')
br = tf.TransformBroadcaster()
rate = rospy.Rate(10)
radius = 5
angle = 0
dist = 3
while not rospy.is_shutdown():
scan = LaserScan()
scan.header.frame_id = "/base_link"
scan.header.stamp = rospy.Time.now()
scan.angle_min = 3.14159274101
scan.angle_max = 3.14159274101
scan.angle_increment = float('nan')
scan.time_increment = 0.0
scan.range_min = 0.04
def bcast(tran_vec, rot_quaternion, parent_frame, child_frame):
br = tf.TransformBroadcaster()
rate = rospy.Rate(10)
br.sendTransform((tran_vec[0], tran_vec[1], tran_vec[2]), rot_quaternion,
rospy.Time.now(), child_frame, parent_frame)
def handle_turtle_pose(msg, turtlename):
br = tf.TransformBroadcaster()
br.sendTransform((msg.x, msg.y,0), tf.transformations.quaternion_from_euler(0, 0, msg.theta), rospy.Time.now(), turtlename, "world")
if __name__ == '__main__':
# Don't change this code####################################################################################
rospy.init_node('sample_node')
####################################################################################################################
relaxedIK = RelaxedIK.init_from_config(config_file_name)
####################################################################################################################
urdf_file = open(relaxedIK.vars.urdf_path, 'r')
urdf_string = urdf_file.read()
rospy.set_param('robot_description', urdf_string)
js_pub = rospy.Publisher('joint_states', JointState, queue_size=5)
ee_pose_goals_pub = rospy.Publisher('/relaxed_ik/ee_pose_goals',
EEPoseGoals,
queue_size=3)
tf_pub = tf.TransformBroadcaster()
rospy.sleep(0.3)
# Don't change this code ###########################################################################################
uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)
roslaunch.configure_logging(uuid)
launch_path = os.path.dirname(
__file__) + '/../launch/robot_state_pub.launch'
launch = roslaunch.parent.ROSLaunchParent(uuid, [launch_path])
launch.start()
####################################################################################################################
rospy.sleep(1.0)
counter = 0.0
def amcl2robot_pose_pub():
global tag_center_pose
global amcl_pose
global orient
global startstop
global th_laser
# si iscrive al topic che contiene la posa del punto centrale tra le due tag
sub_tag_in_map = rospy.Subscriber(tag_in_map_topic_ID, PoseStamped,
vicon_cb_0)
sub_amcl_pose = rospy.Subscriber("amcl_pose", PoseWithCovarianceStamped,
vicon_cb_1)
sub_amcl_pose = rospy.Subscriber("/scan", LaserScan, lidar_cb)
sub_start_stop = rospy.Subscriber("/start_and_stop", Float64, motori_cb)
robot_pose_pub = rospy.Publisher("/robot_pose", Point, queue_size=100)
pub_start_stop = rospy.Publisher("/start_and_stop",
Float64,
queue_size=100)
rospy.init_node('amcl2robot_pose_PY')
while not rospy.is_shutdown():
# controlla quanto la posa di AMCL si discosta da quella delle UWB, tutto nel frame MAP
amcl_x = amcl_pose.pose.pose.position.x
amcl_y = amcl_pose.pose.pose.position.y
amcl_orient = tf.transformations.euler_from_quaternion(
(amcl_pose.pose.pose.orientation.x,
amcl_pose.pose.pose.orientation.y,
amcl_pose.pose.pose.orientation.z,
amcl_pose.pose.pose.orientation.w))
amcl_orient = amcl_orient[2] # prendo lo yaw
# punto centrale tra le due tag
tag_x = tag_center_pose.pose.position.x
tag_y = tag_center_pose.pose.position.y
tag_orient = tf.transformations.euler_from_quaternion(
(tag_center_pose.pose.orientation.x,
tag_center_pose.pose.orientation.y,
tag_center_pose.pose.orientation.z,
tag_center_pose.pose.orientation.w))
tag_orient = tag_orient[2] # prendo lo yaw
if (np.sum(scansione.intensities) <= th_laser):
blind_laser = True
else:
blind_laser = False
robot_pose = Point()
if (blind_laser):
#controllo_precedente = startstop
#pub_start_stop.publish(0.0)
robot_pose.x = tag_x
robot_pose.y = tag_y
robot_pose.z = tag_orient
rospy.loginfo("Laser offline, navigazione UWB in corso")
else:
robot_pose.x = amcl_x
robot_pose.y = amcl_y
robot_pose.z = amcl_orient
robot_pose_pub.publish(robot_pose)
# -------------------------- TF --------------------------
tf_quat = tf.transformations.quaternion_from_euler(0, 0, robot_pose.z)
# publish TF
br_0 = tf.TransformBroadcaster()
br_0.sendTransform((robot_pose.x, robot_pose.y, 0), tf_quat,
rospy.Time.now(), robot_pose_frame_ID, map_frame_ID)
def get_transform_lists(bag_file_name):
init_node()
broadcaster = tf.TransformBroadcaster()
listener = tf.TransformListener()
listener.setUsingDedicatedThread(True)
checkerboard_in_camera_trans = []
wrist_in_body_trans = []
#pdb.set_trace()
camera_in_body_estimate = None
checkerboard_to_wrist_estimate = None
bag = rosbag.Bag(bag_file_name)
tf_header_ids = set()
tf_child_ids = set()
for topic, message, time in bag.read_messages(topics=['tf', '/tf']):
for tf_message_stamped in message.transforms:
tf_message = tf_message_stamped.transform
translation = [
tf_message.translation.x, tf_message.translation.y,
tf_message.translation.z
]
rotation = [
tf_message.rotation.x, tf_message.rotation.y,
tf_message.rotation.z, tf_message.rotation.w
]
broadcaster.sendTransform(translation, rotation, rospy.Time.now(),
tf_message_stamped.child_frame_id,
tf_message_stamped.header.frame_id)
tf_message_stamped.header.stamp = rospy.Time.now()
listener.setTransform(tf_message_stamped, "user")
for tf_message in message.transforms:
if tf_message.header.frame_id not in tf_header_ids:
tf_header_ids.add(tf_message.header.frame_id)
print 'found new frame %s' % tf_message.header.frame_id
if tf_message.child_frame_id not in tf_child_ids:
tf_child_ids.add(tf_message.child_frame_id)
print 'found new child frame %s' % tf_message.child_frame_id
if 'wrist_board' in tf_message.header.frame_id or 'wrist_board_corner' in tf_message.child_frame_id:
print 'found keyframe'
if camera_in_body_estimate is None:
try:
listener.waitForTransform('/camera_link', '/world',
rospy.Time(0),
rospy.Duration(.01))
camera_in_body_tf = listener.lookupTransform(
'/camera_link', '/world', rospy.Time(0))
print "got camera to world transform"
camera_in_body_estimate = tf_conversions.toMatrix(
tf_conversions.fromTf(camera_in_body_tf))
except:
print 'could not get camera to world transform, skipping. Are you sure you ran tf between camera_link and world?'
continue
print "got camera to world estimate"
if checkerboard_to_wrist_estimate is None:
try:
listener.waitForTransform('/wrist_board',
'/wrist_board_corner',
rospy.Time(0),
rospy.Duration(.01))
#(trans, rot) = listener.lookupTransform('/wrist_board','/wrist_board_corner',rospy.Time(0))
#print trans
#print rot
checkerboard_to_wrist_tf = listener.lookupTransform(
'/wrist_board', '/wrist_board_corner',
rospy.Time(0))
#print checkerboard_to_wrist_tf
#raw_input("press a key")
#checkerboard_to_wrist_tf = ((1.75, -0.75, -0.121),(-0.09, -0.04, 0.73, 0.66))
#print 'yinxiao test'
print "got wristboad in wrist"
checkerboard_to_wrist_estimate = tf_conversions.toMatrix(
tf_conversions.fromTf(checkerboard_to_wrist_tf))
except:
print 'could not get wristboard to wrist_board_corner, skipping'
continue
print "got wristboard in wrist estimate"
try:
listener.waitForTransform('/wrist_board', '/camera_link',
rospy.Time(0),
rospy.Duration(.01))
listener.waitForTransform('/wrist_board_corner', '/world',
rospy.Time(0),
rospy.Duration(.1))
checkerboard_tf = listener.lookupTransform(
'/wrist_board', '/camera_link', rospy.Time(0))
#print "got wristboard in camera"
checkerboard_in_camera_trans.append(
tf_conversions.toMatrix(
tf_conversions.fromTf(checkerboard_tf)))
#print "got left wrist in world"
wrist_in_body_tf = listener.lookupTransform(
'/wrist_board_corner', '/world', rospy.Time(0))
wrist_in_body_trans.append(
tf_conversions.toMatrix(
tf_conversions.fromTf(wrist_in_body_tf)))
except:
continue
#print "finished loop"
return checkerboard_in_camera_trans, wrist_in_body_trans, camera_in_body_estimate, checkerboard_to_wrist_estimate
def talker():
odom_pub = rospy.Publisher("odom1", Odometry, queue_size=50)
map_pub = rospy.Publisher("map1", OccupancyGrid, queue_size=10)
rospy.init_node('talker', anonymous=True)
odom_broadcaster = tf.TransformBroadcaster()
#x = 0.0
#y = 0.0
th = 0.0
#vx = 0.1
#vy = -0.1
vth = 0.1
current_time = rospy.Time.now()
last_time = rospy.Time.now()
prefix = "out_turtle_map_"
ext = ".owl"
times = "1713" ## update init owl file
file_input = prefix + times + ext
r = rospy.Rate(1.0)
while not rospy.is_shutdown():
current_time = rospy.Time.now()
g = Graph()
'''
#to Cloud
print "file to download "+ file_input
myCmd = 'gsutil -m cp gs://slam-up-bucket/'+file_input+' ./input'
os.system(myCmd)
local_file_input = 'input/'+file_input
print "file que se va parsear "+local_file_input
'''
local_file_input = 'out/' + file_input ## INFO:: en el local
if os.path.exists(local_file_input):
print "exist path " + local_file_input
g.parse(local_file_input, format="turtle")
print("graph has %s statements." % len(g))
px, py, pz = get_position(g)
ox, oy, oz, ow = get_orientation(g)
matrix = get_covar_matrix(g)
covar = np.array([0.0] * 36).reshape(6, 6)
for cell in matrix:
row = int(cell[0])
col = int(cell[1])
value = float(cell[2])
covar[row, col] = value
covar_list = tuple(covar.ravel().tolist())
#Creacion de nav_msg/Odometry
# compute odometry in a typical way given the velocities of the robot
'''dt = (current_time - last_time).to_sec()
delta_x = (vx * cos(th) - vy * sin(th)) * dt
delta_y = (vx * sin(th) + vy * cos(th)) * dt
delta_th = vth * dt
x += delta_x
y += delta_y
th += delta_th '''
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, th)
# first, we'll publish the transform over tf
odom_broadcaster.sendTransform((px, py, pz), odom_quat,
current_time, "base_link1", "odom1")
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom1"
# set the position
odom.pose.pose = Pose(Point(px, py, pz),
Quaternion(ox, oy, oz, ow))
odom.pose.covariance = covar_list
# set the velocity
vx = vy = 0
odom.child_frame_id = "base_link1"
odom.twist.twist = Twist(Vector3(vx, vy, 0), Vector3(0, 0, vth))
odom_pub.publish(odom)
##map
width = height = resolution = 0
mapa_info = get_map_info(g)
if len(mapa_info) > 0:
flagMap = True
if flagMap is not True and var_m is not None and var_grid_list is not None:
print "using saved"
m = var_m
grid_list = var_grid_list
else:
print "reading map"
for row in mapa_info:
p = row[1]
o = p[p.find('#') + 1:len(p)]
if o == "Width":
width = int(row[2])
if o == "Height":
height = int(row[2])
resolution = get_map_resolution(g)
#Creacion de nav_msg/occupancyGrid
len_grid = width * height
grid_map = np.array([-1] * len_grid).reshape(width, height)
objects = get_map_objects(g)
print("objects detected")
print(len(objects))
for row in objects:
p = row[0]
obj_prefix = "http://github.com/Alex23013/slam-up/individual/obj/"
obj_pos = p[len(obj_prefix):len(p)]
parts = obj_pos.split('_')
grid_map[int(parts[0]), int(parts[1])] = int(row[1])
grid_list = tuple(grid_map.ravel().tolist())
map_broadcaster = tf.TransformBroadcaster()
map_broadcaster.sendTransform(
(0, 0, 0), odom_quat, current_time, "base_link1", "map1")
m = MapMetaData()
m.resolution = resolution
m.width = width
m.height = height
pos = np.array(
[-width * resolution / 2, -height * resolution / 2, 0])
quat = np.array([0, 0, 0, 1])
m.origin = Pose()
m.origin.position.x, m.origin.position.y = pos[:2]
ogrid = OccupancyGrid()
ogrid.header.frame_id = 'map1'
ogrid.header.stamp = rospy.Time.now()
ogrid.info = m
ogrid.data = grid_list
var_m = m
var_grid_list = grid_list
map_pub.publish(ogrid)
rospy.loginfo("publishing map:")
rospy.loginfo(times)
last_time = current_time
else:
print local_file_input, "file not found"
rospy.loginfo("publishing odometry:")
rospy.loginfo(times)
times = str(1 + int(times))
file_input = prefix + times + ext
print "antes sleep"
r.sleep()
print "fin_loop"
def __init__(self):
rospy.init_node('ginger_control_interface')
self.rospy = rospy
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
# the frequency for rate, suppose to under 20 Hz
self.rate = rospy.Rate(20)
# self._main_body_joint = Queue.Queue(2)
# self._left_arm_joint = Queue.Queue(2)
# self._right_arm_joint = Queue.Queue(2)
# self._left_arm_vel = Queue.Queue(2)
# self._right_arm_vel = Queue.Queue(2)
# self._left_arm_cur = Queue.Queue(2)
# self._right_arm_cur = Queue.Queue(2)
#
# self._left_hand_joint = Queue.Queue(2)
# self._right_hand_joint = Queue.Queue(2)
self._main_body_joint = None
self._left_arm_joint = None
self._right_arm_joint = None
self._left_arm_vel = None
self._right_arm_vel = None
self._left_arm_cur = None
self._right_arm_cur = None
self._left_hand_joint = None
self._right_hand_joint = None
################## Subscriber ############################
self.rospy.Subscriber("/MainBody/Position", BodyMsgs,
self.main_body_joint_CALLBACK)
self.rospy.Subscriber("/LeftArm/Position", ArmMsgs,
self.left_arm_joint_CALLBACK)
self.rospy.Subscriber("/RightArm/Position", ArmMsgs,
self.right_arm_joint_CALLBACK)
self.rospy.Subscriber("/LeftArm/Velocity", ArmMsgs,
self.left_arm_vel_CALLBACK)
self.rospy.Subscriber("/RightArm/Velocity", ArmMsgs,
self.right_arm_vel_CALLBACK)
self.rospy.Subscriber("/LeftArm/Current", ArmMsgs,
self.left_arm_cur_CALLBACK)
self.rospy.Subscriber("/RightArm/Current", ArmMsgs,
self.right_arm_cur_CALLBACK)
self.rospy.Subscriber("/LeftHand/Position", HandMsgs,
self.left_hand_joint_CALLBACK)
self.rospy.Subscriber("/RightHand/Position", HandMsgs,
self.right_hand_joint_CALLBACK)
#################### Publisher ###########################
self._main_body_mode_pub = self.rospy.Publisher(
"/XR1/MainBodyChainModeChange", ChainModeChange, queue_size=5)
self._left_arm_mode_pub = self.rospy.Publisher(
"/XR1/LeftArmChainModeChange", ChainModeChange, queue_size=5)
self._right_arm_mode_pub = self.rospy.Publisher(
"/XR1/RightArmChainModeChange", ChainModeChange, queue_size=5)
self._left_hand_mode_pub = self.rospy.Publisher(
"/XR1/LeftHandChainModeChange", ChainModeChange, queue_size=5)
self._right_hand_mode_pub = self.rospy.Publisher(
"/XR1/RightHandChainModeChange", ChainModeChange, queue_size=5)
####
self._main_body_joint_pub = self.rospy.Publisher(
"/MainBody/TargetPosition", BodyMsgs, queue_size=5)
self._left_arm_joint_pub = self.rospy.Publisher(
"/LeftArm/TargetPosition", ArmMsgs, queue_size=5)
self._right_arm_joint_pub = self.rospy.Publisher(
"/RightArm/TargetPosition", ArmMsgs, queue_size=5)
self._left_arm_vel_pub = self.rospy.Publisher(
"/LeftArm/TargetVelocity", ArmMsgs, queue_size=5)
self._right_arm_vel_pub = self.rospy.Publisher(
"/RightArm/TargetVelocity", ArmMsgs, queue_size=5)
self._left_arm_cur_pub = self.rospy.Publisher("/LeftArm/TargetCurrent",
ArmMsgs,
queue_size=5)
self._right_arm_cur_pub = self.rospy.Publisher(
"/RightArm/TargetCurrent", ArmMsgs, queue_size=5)
self._left_hand_joint_pub = self.rospy.Publisher(
"/LeftHand/TargetPosition", HandMsgs, queue_size=5)
self._right_hand_joint_pub = self.rospy.Publisher(
"/RightHand/TargetPosition", HandMsgs, queue_size=5)
self._left_elbow_pub = self.rospy.Publisher("/LeftArm/ElbowAngle",
Float64,
queue_size=5)
self._right_elbow_pub = self.rospy.Publisher("/RightArm/ElbowAngle",
Float64,
queue_size=5)
self._joint_state_pub = self.rospy.Publisher("/ginger/joint_states",
JointState,
queue_size=5)
### uploading joint state to MMO server
self.joint_state = JointState()
self._wheel_name = ["Wheel_left", "Wheel_right", "Wheel_back"]
self._main_body_name = [
"Knee_X", "Back_Z", "Back_X", "Back_Y", "Neck_Z", "Neck_X",
"Head_Y"
]
self._left_arm_name = [
"Left_Shoulder_X", "Left_Shoulder_Y", "Left_Elbow_Z",
"Left_Elbow_X", "Left_Wrist_Z", "Left_Wrist_X", "Left_Wrist_Y"
]
self._right_arm_name = [
"Right_Shoulder_X", "Right_Shoulder_Y", "Right_Elbow_Z",
"Right_Elbow_X", "Right_Wrist_Z", "Right_Wrist_X", "Right_Wrist_Y"
]
self._left_hand_name = [
"Left_1_1", "Left_2_1", "Left_3_1", "Left_4_1", "Left_5_1",
"Left_1_2", "Left_2_2", "Left_3_2", "Left_4_2", "Left_5_2",
"Left_1_3", "Left_2_3", "Left_3_3", "Left_4_3", "Left_5_3"
]
self._right_hand_name = [
"Right_1_1", "Right_2_1", "Right_3_1", "Right_4_1", "Right_5_1",
"Right_1_2", "Right_2_2", "Right_3_2", "Right_4_2", "Right_5_2",
"Right_1_3", "Right_2_3", "Right_3_3", "Right_4_3", "Right_5_3"
]
self.joint_state.name = self._wheel_name + self._main_body_name + self._left_arm_name + self._right_arm_name + \
self._left_hand_name + self._right_hand_name
self.joint_state.position = [0] * 54
self.joint_state.velocity = [0] * 54
self.joint_state.effort = [0] * 54
self.rospy.loginfo("Ginger Control")
def __init__(self):
# Read parameters to configure the node
tf_publish_rate = read_parameter('~tf_publish_rate', 50.)
tf_period = 1./tf_publish_rate if tf_publish_rate > 0 else float('inf')
parent_frame = read_parameter('~parent_frame', 'world')
optitrack_frame = read_parameter('~optitrack_frame', 'optitrack')
rigid_bodies = read_parameter('~rigid_bodies', dict())
names = []
ids = []
for name,value in rigid_bodies.items():
names.append(name)
ids.append(value)
# Setup Publishers
pose_pub = rospy.Publisher('/optitrack/rigid_bodies', RigidBodyArray, queue_size=3)
pose_act_pub = rospy.Publisher('/optitrack/odometry/act_pose', Odometry, queue_size=3)
# pose_act_pub = rospy.Publisher('/optitrack/odometry/act_pose', Odometry, queue_size=3)
# Setup TF listener and broadcaster
tf_listener = tf.TransformListener()
tf_broadcaster = tf.TransformBroadcaster()
# Connect to the optitrack system
iface = read_parameter('~iface', 'eth1')
version = (2, 7, 0, 0) # the latest SDK version
#IPython.embed()
#optitrack = rx.mkdatasock(ip_address=get_ip_address(iface))
#Modified by Nikhil
# optitrack = rx.mkdatasock(ip_address=iface)
optitrack = rx.mkcmdsock(ip_address=iface)
msg = struct.pack("I", rx.NAT_PING)
server_address = '192.168.1.147'
result = optitrack.sendto(msg, (server_address, rx.PORT_COMMAND))
rospy.loginfo('Successfully connected to optitrack')
# Get transformation from the world to optitrack frame
(parent, child) = (parent_frame, optitrack_frame)
try:
now = rospy.Time.now() + rospy.Duration(1.0)
tf_listener.waitForTransform(parent, child, now, rospy.Duration(5.0))
(pos,rot) = tf_listener.lookupTransform(parent, child, now)
wTo = PoseConv.to_homo_mat(pos, rot) # return 4x4 numpy mat
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logwarn('Failed to get transformation from %r to %r frame' % (parent, child))
parent_frame = optitrack_frame
wTo = np.eye(4)
# Track up to 24 rigid bodies
prevtime = np.ones(24)*rospy.get_time()
# IPython.embed()
while not rospy.is_shutdown():
try:
data = optitrack.recv(rx.MAX_PACKETSIZE)
# IPython.embed()
# data, address = optitrack.recvfrom(rx.MAX_PACKETSIZE + 4)
# IPython.embed()
# rospy.loginfo("here")
except socket.error:
if rospy.is_shutdown(): # exit gracefully
return
else:
rospy.logwarn('Failed to receive packet from optitrack')
msgtype, packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
rospy.loginfo('NatNet version received: ' + str(version))
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
# Optitrack gives the position of the centroid.
array_msg = RigidBodyArray()
pose_act_msg = Odometry()
# IPython.embed()
if msgtype == rx.NAT_FRAMEOFDATA:
# loop through all the rigid bodies
# use only one for right now
for i, rigid_body in enumerate(packet.rigid_bodies):
body_id = rigid_body.id
# IPython.embed()
pos_opt = np.array(rigid_body.position)
rot_opt = np.array(rigid_body.orientation)
# IPython.embed()
oTr = PoseConv.to_homo_mat(pos_opt, rot_opt)
# Transformation between world frame and the rigid body
WTr = np.dot(wTo, oTr)
wTW = np.array([[0,-1,0,0],[1,0,0,0],[0,0,1,0],[0,0,0,1]])
wTr = np.dot(wTW,WTr)
## New Change ##
# Toffset = np.array( [[0, 1, 0, 0],[0, 0, 1, 0],[1, 0, 0, 0],[0, 0, 0, 1]] )
# tf_wTr = np.dot(oTr,Toffset)
# tf_pose = Pose()
# tf_pose.position = Point(*tf_wTr[:3,3])
# tf_pose.orientation = Quaternion(*tr.quaternion_from_matrix(tf_wTr))
# IPython.embed()
array_msg.header.stamp = rospy.Time.now()
array_msg.header.frame_id = parent_frame
body_msg = RigidBody()
pose = Pose()
pose.position = Point(*wTr[:3,3])
# OTr = np.dot(oTr,Toffset)
# pose.orientation = Quaternion(*tr.quaternion_from_matrix(oTr))
# change 26 Feb. 2017
# get the euler angle we want then compute the new quaternion
euler = tr.euler_from_quaternion(rot_opt)
quaternion = tr.quaternion_from_euler(euler[2],euler[0],euler[1])
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
# change made 11 july, 2017
# directly get position and orientation information
roll = euler[2]*180/np.pi
pitch = euler[0]*180/np.pi
yaw = euler[1]*180/np.pi
# set up pose_act_msg publisher
pose_act_msg.header.stamp = array_msg.header.stamp
pose_act_msg.header.frame_id = parent_frame
# add velocity estimation later
# pose_act_msg.vel
pose_act_msg.position = pose.position
pose_act_msg.yaw = yaw
pose_act_msg.euler.x = roll
pose_act_msg.euler.y = pitch
pose_act_msg.euler.z = yaw
pose_act_msg.tracking_valid = rigid_body.tracking_valid
body_msg.id = body_id
body_msg.tracking_valid = rigid_body.tracking_valid
body_msg.mrk_mean_error = rigid_body.mrk_mean_error
body_msg.pose = pose
for marker in rigid_body.markers:
# TODO: Should transform the markers as well
body_msg.markers.append(Point(*marker))
array_msg.bodies.append(body_msg)
# Control the publish rate for the TF broadcaster
if rigid_body.tracking_valid and (rospy.get_time()-prevtime[body_id] >= tf_period):
body_name = 'rigid_body_%d' % (body_id)
# if body_id in ids:
# idx = ids.index(body_id)
# body_name = names[idx]
## no change ##
# tf_broadcaster.sendTransform(pos_opt, rot_opt, rospy.Time.now(), body_name, optitrack_frame)
# change 1 ##
# pos2 = np.array([tf_pose.position.x, tf_pose.position.y, tf_pose.position.z])
# rot2 = np.array([tf_pose.orientation.x, tf_pose.orientation.y, tf_pose.orientation.z, tf_pose.orientation.w])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 2 ## <fail>
# pos2 = np.array([-pose.position.y, pose.position.x, pose.position.z])
# rot2 = np.array([-pose.orientation.y, pose.orientation.x, pose.orientation.z, pose.orientation.w])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 3 ## <fail>
# pos2 = np.array([-pos_opt[1],pos_opt[0],pos_opt[2]])
# rot2 = np.array([-rot_opt[1],rot_opt[0],rot_opt[2],rot_opt[3]])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 4 ## <>
pos2 = np.array([pose.position.x, pose.position.y, pose.position.z])
rot2 = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z,pose.orientation.w])
tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, parent_frame)
prevtime[body_id] = rospy.get_time()
pose_pub.publish(array_msg)
pose_act_pub.publish(pose_act_msg)
def publish_tf(self): br = tf.TransformBroadcaster() br.sendTransform((self.model_pose1[0], self.model_pose1[1], self.model_pose1[2]), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "object", "odom")
def __init__(self):
# parameters
self.ANGLE_STEP = int(rospy.get_param("~angle_step"))
self.MAX_PARTICLES = int(rospy.get_param("~max_particles"))
self.MAX_VIZ_PARTICLES = int(rospy.get_param("~max_viz_particles"))
self.INV_SQUASH_FACTOR = 1.0 / float(rospy.get_param("~squash_factor"))
self.MAX_RANGE_METERS = float(rospy.get_param("~max_range"))
self.THETA_DISCRETIZATION = int(
rospy.get_param("~theta_discretization"))
self.WHICH_RM = rospy.get_param("~range_method", "cddt").lower()
self.RANGELIB_VAR = int(rospy.get_param("~rangelib_variant", "3"))
self.SHOW_FINE_TIMING = bool(rospy.get_param("~fine_timing", "0"))
self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1"))
self.PUBLISH_TF = bool(rospy.get_param("~publish_tf", "1"))
self.DO_VIZ = bool(rospy.get_param("~viz"))
# sensor model constants
self.Z_SHORT = float(rospy.get_param("~z_short", 0.01))
self.Z_MAX = float(rospy.get_param("~z_max", 0.07))
self.Z_RAND = float(rospy.get_param("~z_rand", 0.12))
self.Z_HIT = float(rospy.get_param("~z_hit", 0.75))
self.SIGMA_HIT = float(rospy.get_param("~sigma_hit", 8.0))
# motion model constants
self.MOTION_DISPERSION_X = float(
rospy.get_param("~motion_dispersion_x", 0.05))
self.MOTION_DISPERSION_Y = float(
rospy.get_param("~motion_dispersion_y", 0.025))
self.MOTION_DISPERSION_THETA = float(
rospy.get_param("~motion_dispersion_theta", 0.25))
# various data containers used in the MCL algorithm
self.MAX_RANGE_PX = None
self.odometry_data = np.array([0.0, 0.0, 0.0])
self.laser = None
self.iters = 0
self.map_info = None
self.map_initialized = False
self.lidar_initialized = False
self.odom_initialized = False
self.last_pose = None
self.laser_angles = None
self.downsampled_angles = None
self.range_method = None
self.last_time = None
self.last_stamp = None
self.first_sensor_update = True
self.state_lock = Lock()
# cache this to avoid memory allocation in motion model
self.local_deltas = np.zeros((self.MAX_PARTICLES, 3))
# cache this for the sensor model computation
self.queries = None
self.ranges = None
self.tiled_angles = None
self.sensor_model_table = None
# particle poses and weights
self.inferred_pose = None
self.particle_indices = np.arange(self.MAX_PARTICLES)
self.particles = np.zeros((self.MAX_PARTICLES, 3))
self.weights = np.ones(self.MAX_PARTICLES) / float(self.MAX_PARTICLES)
# initialize the state
self.smoothing = Utils.CircularArray(10)
self.timer = Utils.Timer(10)
self.get_omap()
self.precompute_sensor_model()
self.initialize_global()
# these topics are for visualization
self.pose_pub = rospy.Publisher("/pf/viz/inferred_pose",
PoseStamped,
queue_size=1)
self.particle_pub = rospy.Publisher("/pf/viz/particles",
PoseArray,
queue_size=1)
self.pub_fake_scan = rospy.Publisher("/pf/viz/fake_scan",
LaserScan,
queue_size=1)
self.rect_pub = rospy.Publisher("/pf/viz/poly1",
PolygonStamped,
queue_size=1)
if self.PUBLISH_ODOM:
self.odom_pub = rospy.Publisher("/pf/pose/odom",
Odometry,
queue_size=1)
# these topics are for coordinate space things
self.pub_tf = tf.TransformBroadcaster()
# these topics are to receive data from the racecar
self.laser_sub = rospy.Subscriber(rospy.get_param(
"~scan_topic", "/scan"),
LaserScan,
self.lidarCB,
queue_size=1)
self.odom_sub = rospy.Subscriber(rospy.get_param(
"~odometry_topic", "/odom"),
Odometry,
self.odomCB,
queue_size=1)
self.pose_sub = rospy.Subscriber("/initialpose",
PoseWithCovarianceStamped,
self.clicked_pose,
queue_size=1)
self.click_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_pose,
queue_size=1)
print "Finished initializing, waiting on messages..."
def usbl_move(pos):
broadcaster = tf.TransformBroadcaster()
if usbl_move.start:
dt = 2
loc = np.matrix([
[0], #v_x
[0], #v_y
[pos.pose.position.x], #x
[pos.pose.position.y]
]) #z
A = np.matrix([[
1,
0,
0,
0,
], [
0,
1,
0,
0,
], [
dt,
0,
1,
0,
], [
0,
dt,
0,
1,
]])
B = np.matrix([0])
C = np.eye(loc.shape[0])
Q = np.eye(loc.shape[0]) * 0.5
R = np.eye(loc.shape[0]) * 5000
P = np.eye(loc.shape[0])
U = np.matrix([[0]])
Z = np.matrix([[0], [0], [0], [0]])
usbl_move.kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, loc)
usbl_move.md_filter = md_filter.md_filter(2, [1.75, 1.6], 10, [0, 1])
usbl_move.start = 0
if usbl_move.md_filter.update([pos.pose.position.x, pos.pose.position.y]):
#update.ax.scatter(pos.position.x,pos.position.y,-1*current.position.z,color='b')
current.position.x = pos.pose.position.x
current.position.y = pos.pose.position.y
#current.position.z = pos.pose.position.z
#update('b')
Z = np.matrix([[0], [0], [pos.pose.position.x], [pos.pose.position.y]])
U = np.matrix([[0]])
usbl_move.kalman.move(U, Z)
kalman_pos = usbl_move.kalman.getState()
current.position.y = kalman_pos[2]
current.position.x = kalman_pos[3]
broadcaster.sendTransform(
(current.position.x, current.position.y, current.position.z),
(current.orientation.x, current.orientation.y, current.orientation.z,
current.orientation.w), rospy.Time.now(), "body", "odom")
def talker(self):
rospy.Subscriber(self.imu_topic, Imu, self.imu_callback)
odom_broadcaster = tf.TransformBroadcaster()
odom_pub = rospy.Publisher(self.odom_topic, Odometry, queue_size=10)
depth_pub = rospy.Publisher(self.depth_topic, Range, queue_size=10)
temperature_pub = rospy.Publisher(self.temperature_topic,
Temperature,
queue_size=10)
rospy.init_node('sonar_publisher', anonymous=True)
odom_msg = Odometry()
odom_quart = Quaternion()
depth_msg = Range()
temperature_msg = Temperature()
odom_msg.pose.covariance = [0] * 36
odom_msg.twist.covariance = [0] * 36
r = rospy.Rate(20)
self.x_pos = 0.0
self.y_pos = 0.0
self.theta = 0.0
while not rospy.is_shutdown():
# get linear velocity
data = self.DST.next()
# print data
if data is not None:
# print data
for key, value in data.iteritems():
if key == "speed":
self.v_x = value
rospy.loginfo("speed is %f", self.v_x)
elif key == "depth":
self.depth = value
rospy.loginfo("depth is %f", self.depth)
elif key == "temperature":
self.temperature = value
rospy.loginfo("temperature is %f", self.temperature)
else:
rospy.loginfo("data received is invalid")
else:
rospy.loginfo("no data is reveived")
# get current time
self.current_time = rospy.Time.now()
# change in time
self.DTT = (self.current_time - self.last_time).to_sec()
rospy.loginfo("time is %f", self.DTT)
# angular displacement
self.delta_theta = self.imu_z * self.DTT
# linear displacement in x
self.delta_x = (self.v_x * math.cos(self.theta)) * self.DTT
# linear displacement in y
self.delta_y = (self.v_x * math.sin(self.theta)) * self.DTT
# current position
self.x_pos += self.delta_x
self.y_pos += self.delta_y
self.theta += self.delta_theta
# calculate heading in quaternion
# rotate z upside down
odom_quart = tf.transformations.\
quaternion_from_euler(math.pi, 0, self.theta)
# rospy.loginfo(odom_quart)
odom_broadcaster.sendTransform((self.x_pos, self.y_pos, 0),
odom_quart, rospy.Time.now(),
self.fixed_frame, self.odom_frame)
odom_msg.header.stamp = self.current_time
odom_msg.header.frame_id = self.odom_frame
odom_msg.pose.pose.position.x = self.x_pos
odom_msg.pose.pose.position.y = self.y_pos
odom_msg.pose.pose.position.z = 0.0
odom_msg.pose.pose.orientation.x = odom_quart[0]
odom_msg.pose.pose.orientation.y = odom_quart[1]
odom_msg.pose.pose.orientation.z = odom_quart[2]
odom_msg.pose.pose.orientation.w = odom_quart[3]
odom_msg.pose.covariance[0] = 0.00001
odom_msg.pose.covariance[7] = 0.00001
odom_msg.pose.covariance[14] = 1000000000000.0
odom_msg.pose.covariance[21] = 1000000000000.0
odom_msg.pose.covariance[28] = 1000000000000.0
odom_msg.pose.covariance[35] = 0.001
odom_msg.child_frame_id = self.fixed_frame
odom_msg.twist.twist.linear.x = self.v_x
odom_msg.twist.twist.linear.y = 0.0
odom_msg.twist.twist.linear.z = 0.0
odom_msg.twist.twist.angular.x = 0.0
odom_msg.twist.twist.angular.y = 0.0
odom_msg.twist.twist.angular.z = self.imu_z
odom_msg.twist.covariance[0] = 0.00001
odom_msg.twist.covariance[7] = 0.00001
odom_msg.twist.covariance[14] = 1000000000000.0
odom_msg.twist.covariance[21] = 1000000000000.0
odom_msg.twist.covariance[28] = 1000000000000.0
odom_msg.twist.covariance[35] = 0.001
depth_msg.radiation_type = Range.ULTRASOUND
depth_msg.header.stamp = self.current_time
depth_msg.header.frame_id = self.odom_frame
depth_msg.field_of_view = 3 * 2 / math.pi
depth_msg.min_range = 0.2
depth_msg.max_range = 80
depth_msg.range = self.depth
temperature_msg.header.stamp = self.current_time
temperature_msg.header.frame_id = self.odom_frame
temperature_msg.temperature = self.temperature
temperature_msg.variance = 0.0
odom_pub.publish(odom_msg)
depth_pub.publish(depth_msg)
temperature_pub.publish(temperature_msg)
self.last_time = self.current_time
r.sleep()
def pose_move(pos):
#pos.position.z is in kPa, has to be convereted to depth
# P = P0 + pgz ----> pos.position.z = P0 + pg*z_real
#z_real = -(1000*pos.position.z-101.325)/9.81
z_real = -pos.position.z
toDegree = 180 / math.pi
current.position.z = z_real
broadcaster = tf.TransformBroadcaster()
#set up the Kalman Filter
#tf.transformations.quaternion_from_euler()
(roll, pitch, yaw) = tf.transformations.euler_from_quaternion([
-1 * pos.orientation.y, pos.orientation.x, -1 * pos.orientation.z,
pos.orientation.w
])
roll = roll * toDegree
pitch = pitch * toDegree
yaw = yaw * toDegree
s = 'The value of roll is ' + repr(roll) + ', and pitch is ' + repr(
pitch) + ', yaw = ' + repr(yaw)
print s
if pose_move.start:
dt = .02
pos = np.matrix([
[0], # v_x
[0], # v_y
[0], # v_z
[roll], # x
[pitch], # y
[yaw]
]) # z
A = np.matrix([[
1,
0,
0,
0,
0,
0,
], [
0,
1,
0,
0,
0,
0,
], [
0,
0,
1,
0,
0,
0,
], [
dt,
0,
0,
1,
0,
0,
], [
0,
dt,
0,
0,
1,
0,
], [
0,
0,
dt,
0,
0,
1,
]])
B = np.matrix([0])
C = np.eye(pos.shape[0])
Q = np.eye(pos.shape[0]) * .5
R = np.eye(pos.shape[0]) * 500
P = np.eye(pos.shape[0])
U = np.matrix([[0]])
Z = np.matrix([[0], [0], [0], [0], [0], [0]])
pose_move.kalman = kalman_filter.kalman_filter(A, B, C, Q, P, R, pos)
pose_move.start = 0
Z = np.matrix([[0], [0], [0], [roll], [pitch], [yaw]])
U = np.matrix([[0]])
pose_move.kalman.move(U, Z)
pos = pose_move.kalman.getState()
roll = pos[3]
pitch = pos[4]
yaw = pos[5]
# quad = tf.transformations.quaternion_from_euler(roll/toDegree, pitch/toDegree, yaw/toDegree )
quad = tf.transformations.quaternion_from_euler(0, 0, yaw / toDegree)
current.orientation.x = quad[0]
current.orientation.y = quad[1]
current.orientation.z = quad[2]
current.orientation.w = quad[3]
broadcaster.sendTransform(
(current.position.x, current.position.y, current.position.z),
(current.orientation.x, current.orientation.y, current.orientation.z,
current.orientation.w), rospy.Time.now(), "body", "odom")
def __init__(self, rate=10):
# self.pub_Image = rospy.Publisher('image_raw_sync', SesnorImage, queue_size=1)
# self.pub_Cam_Info = rospy.Publisher('camera_info_sync', CameraInfo, queue_size=1)
# self.pub_Lidar = rospy.Publisher('rslidar_points_sync', PointCloud2, queue_size=1)
self.imuInput = rospy.Subscriber("/self/usv/imu",
Imu,
self.imuCallback,
queue_size=2)
self.gpsInput = rospy.Subscriber('/self/usv/gps',
NavSatFix,
self.gpsCallback,
queue_size=2)
self.gpsVeloInput = rospy.Subscriber('/self/usv/gps/fix_velocity',
Vector3Stamped,
self.gpsVeloCallback,
queue_size=2)
self.targetGpsInput = rospy.Subscriber('/target/usv_2/gps',
NavSatFix,
self.targetGpsCallback,
queue_size=2)
self.targetGpsVeloInput = rospy.Subscriber(
'/target/usv_2/gps/fix_velocity',
Vector3Stamped,
self.targetGpsVeloCallback,
queue_size=2)
self.sensor_pub = rospy.Publisher('/base/sensor',
BaseSensor,
queue_size=1)
# self.ts = message_filters.TimeSynchronizer([self.imuInput
# , self.gpsInput
# ], 10)
# self.ts.registerCallback(self.general_callback)
self.br_boat_world = tf.TransformBroadcaster()
self.br_lidar_boat = tf.TransformBroadcaster()
self.br_lidar_boat2 = tf.TransformBroadcaster()
self.br_lidar_boat3 = tf.TransformBroadcaster()
self.br_camera_boat = tf.TransformBroadcaster()
self.gps_velo_msg = Vector3Stamped()
self.target_gps_msg = NavSatFix()
self.target_gps_velo_msg = Vector3Stamped()
self.imuflag = False
self.gpsflag = False
self.lat = d2r(30.06022459407145675)
self.lon = d2r(120.173913575780311191)
rate = rospy.Rate(rate)
while not rospy.is_shutdown():
if self.imuflag and self.gpsflag:
print('process')
self.general_callback(self.imu_msg, self.gps_msg,
self.gps_velo_msg, self.target_gps_msg,
self.target_gps_velo_msg)
self.imuflag = False
self.gpsflag = False
rate.sleep()
def main():
rospy.init_node('leica_tf_broadcaster', anonymous=True)
while (not rospy.is_shutdown()):
PrismLowerLeftGate_broadcaster1 = tf.TransformBroadcaster()
PrismTopGate_broadcaster2 = tf.TransformBroadcaster()
PrismLowerRightGate_broadcaster3 = tf.TransformBroadcaster()
RobotLeftPrism_broadcaster1 = tf.TransformBroadcaster()
RobotTopPrism_broadcaster2 = tf.TransformBroadcaster()
RobotRightPrism_broadcaster3 = tf.TransformBroadcaster()
############## Static Transform from Gate Origin to respective prism ##############
#create a quaternion
rotation_quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, 0.0)
#translation vector
#According to Sheet for older terrain
# Prism1_translation_vector1 = (0.0,1.592 , 1.11)
# Prism2_translation_vector2 = (0.0, 0.0, 1.82)
# Prism3_translation_vector3 = (0.0, -1.671, 0.79)
Prism1_translation_vector1 = (0.0, 1.523, 1.07)
Prism2_translation_vector2 = (0.0, 0.0, 1.83)
Prism3_translation_vector3 = (0.0, -1.394, 0.755)
############## Static Transform from Robot_Origin to respective prism ##############
#create a quaternion
RobotLeftPrism_rotation_quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, 0.0)
RobotTopPrism_rotation_quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, 0.0)
RobotRightPrism_rotation_quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, 0.0)
#translation vector
# RobotLeftPrism_translation_vector1 = (-0.2513, 0.2, 0.2833)
# RobotTopPrism_translation_vector2 = (0.0013, -0.2, 0.2833)
# RobotRightPrism_translation_vector3 = (-0.2513, -0.2, 0.2833)
RobotLeftPrism_translation_vector1 = (-0.045, 0.100025, 0.0174602)
RobotTopPrism_translation_vector2 = (0.045, -0.100025, 0.0174602)
RobotRightPrism_translation_vector3 = (-0.045, -0.100025, 0.0174602)
# #For Updated Prism Rig
# RobotLeftPrism_translation_vector1 = (0.3929, 0.100025, 0.5247)
# RobotTopPrism_translation_vector2 = (0.4829, -0.100025, 0.5247)
# RobotRightPrism_translation_vector3 = (0.3929, -0.100025, 0.5247)
#time
current_time = rospy.Time.now()
PrismLowerLeftGate_broadcaster1.sendTransform(
Prism1_translation_vector1, rotation_quaternion, current_time,
"PrismLeftLowerGate", "Gate_Origin") #child frame, parent frame
PrismTopGate_broadcaster2.sendTransform(
Prism2_translation_vector2, rotation_quaternion, current_time,
"PrismTopGate", "Gate_Origin") #child frame, parent frame
PrismLowerRightGate_broadcaster3.sendTransform(
Prism3_translation_vector3, rotation_quaternion, current_time,
"PrismRightLowerGate", "Gate_Origin") #child frame, parent frame
RobotLeftPrism_broadcaster1.sendTransform(
RobotLeftPrism_translation_vector1,
RobotLeftPrism_rotation_quaternion, current_time, "RobotLeftPrism",
"Robot_Origin") #child frame, parent frame
RobotTopPrism_broadcaster2.sendTransform(
RobotTopPrism_translation_vector2,
RobotTopPrism_rotation_quaternion, current_time, "RobotTopPrism",
"Robot_Origin") #child frame, parent frame
RobotRightPrism_broadcaster3.sendTransform(
RobotRightPrism_translation_vector3,
RobotRightPrism_rotation_quaternion, current_time,
"RobotRightPrism", "Robot_Origin") #child frame, parent frame
time.sleep(0.5)
rospy.spin()
def __init__(self):
# Node initiation with log level debug by default
rospy.init_node('simple_aruco_localisation', log_level=rospy.DEBUG)
# IIND Format
rospy.loginfo("--------------------------------------------------")
rospy.loginfo(" Integrated Innovation for Nuclear Decomissioning ")
rospy.loginfo(" Simple Aruco Localisation ")
rospy.loginfo(" partner: Wood ")
rospy.loginfo("--------------------------------------------------")
#> Attributes
#> Parameters
frequency = rospy.get_param('~frequency', 50)
rospy.loginfo("Frequency: %f", frequency)
rate = rospy.Rate(frequency)
self.parent_id = rospy.get_param('~parent_id', 'world')
self.child_id = rospy.get_param('~child_id', 'marker')
self.camera_optical_frame_id = rospy.get_param('~camera_optical_frame_id','i3dr_stereo_cameraLeft_optical')
self.marker_frame_id = rospy.get_param('~marker_frame_id','frame_marker_1')
rospy.loginfo("Frequency: %f", frequency)
camera_frames = [self.camera_optical_frame_id]
marker_frames = [self.marker_frame_id]
tfl = tf.TransformListener()
tft = tf.TransformerROS()
tfb = tf.TransformBroadcaster()
## Loop
while not rospy.is_shutdown():
try:
temps = rospy.Time(0)
ts,oks = [],[False,False]
for i in range(len(camera_frames)):
if tfl.canTransform( camera_frames[i], marker_frames[i], temps): # Should check with fiducial transforms
(t, q) = tfl.lookupTransform(camera_frames[i], marker_frames[i], temps)
ts.append(t)
oks[i] = True
if not oks[0] and not oks[1]:
rospy.logerr_throttle(1,"Transformations not available")
continue
else:
if not oks[1]:
marker_frame = marker_frames[0]
elif not oks[0]:
marker_frame = marker_frames[1]
else:
# Check which marker is closer and choose that location
if np.linalg.norm(ts[0]) <= np.linalg.norm(ts[1]):
marker_frame = marker_frames[0]
else:
marker_frame = marker_frames[1]
rospy.loginfo_throttle(1,marker_frame.split('_')[0])
(t_w_m, q_w_m) = tfl.lookupTransform("world", marker_frame, temps)
tfb.sendTransform(t_w_m, tf.transformations.quaternion_from_euler(q_w_m[0],q_w_m[1],q_w_m[2]), temps, self.child_id, self.parent_id)
except tf.Exception as e:
rospy.logwarn("error while waiting for frames: {0}".format(e))
rate.sleep()
def follow(self):
if not (self.target_found and self.object_location != None):
return
pid = self.camera_pid
pid.kp = -0.0005
pid.kd = 0.005
target_value = 0.0
current_value = self.object_location
print current_value
ang = pid.compute(current_value, target_value)
keep_distance = 0.3
spectrum = 20
mid = np.amin(np.concatenate((self.laser_values[-spectrum:], self.laser_values[:spectrum]), axis=0) )
#right = np.amin(self.laser_values[-2*spectrum:-spectrum])
#left = np.amin(self.laser_values[spectrum:spectrum*2])
#print 'left', left, 'mid', mid, 'right', right, ' ang', ang
if self.target_found:
if abs(current_value)<2:
distance = min(self.laser_values[0],3)
if distance < 0.1:
return
client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
client.wait_for_server()
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "base_link"
goal.target_pose.header.stamp = rospy.Time.now()
# Move 0.5 meters forward along the x axis of the "map" coordinate frame
goal.target_pose.pose.position.x = distance - 0.4
# No rotation of the mobile base frame w.r.t. map frame
goal.target_pose.pose.orientation.w = 1.0
print 'going on goal', goal
time.sleep(3.0)
SimpleActionClient = client.send_goal(goal)
time.sleep(2.0)
client.cancel_goal()
# Waits for the server to finish performing the action.
wait = client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
# Result of executing the action
print 'result', client.get_result()
br = tf.TransformBroadcaster()
br.sendTransform((distance, 0.0, 0.0),(0.0, 0.0, 0.0, 1.0),rospy.Time.now(),"/carrot1","/base_link")
ls = tf.TransformListener()
#print ls.allFramesAsString()
#print 'carrot', ls.frameExists("/carrot1"), 'map', ls.frameExists("map"), 'base_link', ls.frameExists("base_link")
if ls.frameExists("carrot1") and ls.frameExists("map"):
t = ls.getLatestCommonTime("carrot1", "map")
position, quaternion = ls.lookupTransform("carrot1", "map", t)
print 'pos', position
return
if mid<1.5:
#drive(mid - keep_distance ,ang)
vel = mid - keep_distance
else:
#drive(1,ang)
vel = 1
else:
#drive(0,0)
vel = 0
ang = 0
vel=0
self.drive(vel,-ang)
def __init__(self):
rospy.init_node('arduino_relay')
self._imu_data = {}
self._lock = threading.RLock()
# self.imu_calibration_loaded = False
# self.imu_calibration_loaded_time = rospy.Time(0)
self.diagnostics_msg_count = 0
self.diagnostics_buffer = []
# http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
self.tf_br = tf.TransformBroadcaster()
#self.tf2_br = tf2_ros.TransformBroadcaster()
#rospy.Service('~reset_odometry', Empty, self.reset_odometry)
## Publishers.
self.diagnostics_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=10)
self.odometry_pub = rospy.Publisher('/odom', Odometry, queue_size=10)
self.imu_calibration_load_pub = rospy.Publisher(
'/torso_arduino/imu_calibration_load',
UInt16MultiArray,
queue_size=1)
self.imu_pub = rospy.Publisher('/imu_data', Imu, queue_size=10)
self.joint_pub = rospy.Publisher('joint_states',
JointState,
queue_size=10)
self._joint_pub_lock = threading.RLock()
self.last_pan_angle = None
self.last_tilt_angle = None
self.received_angles = False
self._joint_pub_thread = threading.Thread(
target=self.publish_joints_thread)
self._joint_pub_thread.daemon = True
self._joint_pub_thread.start()
## Head Subscribers.
rospy.Subscriber('/head_arduino/diagnostics_relay', String,
partial(self.on_diagnostics_relay, prefix='head'))
rospy.Subscriber('/head_arduino/pan_degrees', Int16,
self.on_head_pan_degrees)
rospy.Subscriber('/head_arduino/tilt_degrees', Int16,
self.on_head_tilt_degrees)
## Torso Subscribers.
rospy.Subscriber('/torso_arduino/diagnostics_relay', String,
partial(self.on_diagnostics_relay, prefix='torso'))
# rospy.Subscriber('/torso_arduino/imu_calibration_save', UInt16MultiArray, self.on_imu_calibration_save)
rospy.Subscriber('/torso_arduino/imu_relay', String, self.on_imu_relay)
rospy.Subscriber('/torso_arduino/odometry_relay', String,
self.on_odometry_relay)
rospy.Subscriber('/torso_arduino/power_shutdown', Bool,
self.on_power_shutdown)
self._odom_lock = threading.RLock()
self.pos = None
self.ori = None
# self._motor_target_left = None
# self._motor_target_right = None
# rospy.Subscriber('/torso_arduino/motor_target_a', Int16, partial(self.on_motor_target, side='left'))
# rospy.Subscriber('/torso_arduino/motor_target_b', Int16, partial(self.on_motor_target, side='right'))
# self._motor_encoder_left = None
# self._motor_encoder_right = None
# rospy.Subscriber('/torso_arduino/motor_encoder_a', Int16, partial(self.on_motor_encoder, side='left'))
# rospy.Subscriber('/torso_arduino/motor_encoder_b', Int16, partial(self.on_motor_encoder, side='right'))
## Begin IO.
rospy.spin()
def __init__(self):
self.br = tf.TransformBroadcaster()
self.tf_send_divider = 10
self.tf_send_count = 0
self.sub_odom = rospy.Subscriber("/proc_navigation/odom", Odometry,
self.odom_callback)
x = 0.0
y = 0.0
theta = 0.0
vx = 0.0
vy = 0.0
vth = 0.0
dc = 0.0
dr = 0.0
dl = 0.0
dt = 0.0
dx = 0.0
dy = 0.0
dtheta = 0.0
current_time = rospy.Time.now()
last_time = rospy.Time.now()
if __name__ == '__main__':
try:
odom_pub = rospy.Publisher("/odom", Odometry, queue_size=50)
tick_sub = rospy.Subscriber("/TicksPublisher", EncoderTick,
tickcallback)
odom_broadcaster = tf.TransformBroadcaster()
rospy.spin()
except rospy.ROSInterruptException:
pass
def __init__(self, reward=NullReward(),
namespace='/costar',
observe=None,
robot_config=None,
lfd=None,
tf_listener=None,
use_default_pose=False,
*args, **kwargs):
super(CostarWorld, self).__init__(reward, *args, **kwargs)
# This is the set of trajectory data we will use for learning later on.
self.trajectories = {}
# This is the set of object information we will use for learning later on
# It tells us which objects/features each individual skill should depend on
# and is used when extracting a set of features.
self.objs = {}
# This is extra data -- such as world state observations -- that is
# associated with our training trajectories.
self.trajectory_data = {}
# This is where we actually store all the information about our learned
# skills.
self.models = {}
# ------------------------- VISUALIZATION TOOLS ---------------------------
# These are publishers for pose arrays that help us visualize data and
# learned actions.
self.traj_pubs = {}
self.traj_data_pubs = {}
self.skill_pubs = {}
self.tf_listener = tf_listener
if self.tf_listener is None:
self.tf_listener = tf.TransformListener()
# Other things
self.observe = observe
self.predicates = []
self.namespace = namespace
# This is the current state of all non-robot objects in the world --
# which is to say, for now, it's just a dictionary of frames by TF frame
# identifier.
self.observation = {}
# Object class information
# TODO(cpaxton): this is a duplicate, remove it after state has been
# fixed a little
self.object_by_class = {}
self.objects_to_track = []
if robot_config is None:
raise RuntimeError('Must provide a robot config!')
elif not isinstance(robot_config, list):
robot_config = [robot_config]
# -------------------------- ROBOT INFORMATION ----------------------------
# Base link, end effector, etc. for easy reference
# set up actors and things
self.tf_pub = tf.TransformBroadcaster()
# Create and add all the robots we want in this world.
for i, robot in enumerate(robot_config):
name = robot['name']
if robot['q0'] is not None:
s0 = CostarState(i,
q=robot['q0'],
dq=np.zeros_like(robot['q0']))
else:
s0 = CostarState(self, i, None, None)
self.addActor(
CostarActor(robot,
state=s0,
dynamics=self.getT(robot),
policy=NullPolicy()))
# -------------------------------------------------------------------------
# For grippers. We check these on a _update_environment() to get the current gripper
# state.
self.gripper_status_listeners = {}
# -------------------------------------------------------------------------
# Visualization helper
self.plan_pub = rospy.Publisher(
join(self.namespace, "plan"),
PoseArray,
queue_size=1000)
# The base link for the scene as a whole
self.base_link = self.actors[0].base_link
if not lfd:
self.lfd = LfD(self.actors[0].config)
else:
self.lfd = lfd
def callbackPoseRCNN(self, data):
# recive data
objArray = Detection2DArray()
# rcnn_pose
objArray = data
# rospy.logdebug(" lenth objArray.detections: %f", len(objArray.detections))
size_filter = 10
if len(objArray.detections) != 0:
# align coordinate axis X
neuralx_current = (
-1) * objArray.detections[0].results[0].pose.pose.position.z
neuraly_current = (
-1) * objArray.detections[0].results[0].pose.pose.position.x
neuralz_current = objArray.detections[0].results[
0].pose.pose.position.y
# rospy.logdebug("--------------------------------")
# rospy.logdebug("rcnn_pose.x (m): %f", VecNeuralx_current)
# rospy.logdebug("rcnn_pose.y (m): %f", VecNeuraly_current)
# rospy.logdebug("rcnn_pose.z (m): %f", neuralz_current)
##############################################################
# Filter list_x
if len(self.list_x) < size_filter:
self.list_x.append(neuralx_current)
# rospy.logdebug("--------------------------------")
# rospy.logdebug('Size of list: %f',len(self.list_x))
# rospy.logdebug('****Incomplete List')
self.VecNeural_x_previous = neuralx_current
else:
diff = abs(neuralx_current - self.VecNeural_x_previous)
# rospy.logdebug('------------------------------')
# rospy.logdebug('Diff points: %f',diff)
if diff > 0.2 and self.VecNeural_x_previous != 0:
self.VecNeural_x_previous = neuralx_current
# self.VecNeural.x = 0
# rospy.logdebug('------------------------------')
# rospy.logdebug('****No capture!')
else:
self.list_x.append(neuralx_current)
del self.list_x[0]
neuralx_current = sum(self.list_x) / len(self.list_x)
self.VecNeural.x = neuralx_current
# rospy.logdebug('------------------------------')
# rospy.logdebug('Size of list_x: %f',len(self.list_x))
# rospy.logdebug('Sum List list_x: %f',sum(self.list_x))
# rospy.logdebug('Med List list_x: %f',self.VecNeural.x)
self.VecNeural_x_previous = neuralx_current
##############################################################
# Filter list_y
if len(self.list_y) < size_filter:
self.list_y.append(neuraly_current)
# rospy.logdebug("--------------------------------")
# rospy.logdebug('Size of list: %f',len(self.list_y))
# rospy.logdebug('****Incomplete List')
self.VecNeural_y_previous = neuraly_current
else:
diff = abs(neuraly_current - self.VecNeural_y_previous)
# rospy.logdebug('------------------------------')
# rospy.logdebug('Diff points: %f',diff)
if diff > 0.2 and self.VecNeural_y_previous != 0:
self.VecNeural_y_previous = neuraly_current
# self.VecNeural.y = 0
# rospy.logdebug('------------------------------')
# rospy.logdebug('****No capture!')
else:
self.list_y.append(neuraly_current)
del self.list_y[0]
neuraly_current = sum(self.list_y) / len(self.list_y)
self.VecNeural.y = neuraly_current
# rospy.logdebug('------------------------------')
# rospy.logdebug('Size of list_y: %f',len(self.list_y))
# rospy.logdebug('Sum List list_y: %f',sum(self.list_y))
# rospy.logdebug('Med List list_y: %f',self.VecNeural.y)
self.VecNeural_y_previous = neuraly_current
##############################################################
# Filter list_z
if len(self.list_z) < size_filter:
self.list_z.append(neuralz_current)
# rospy.logdebug("--------------------------------")
# rospy.logdebug('Size of list: %f',len(self.list_z))
# rospy.logdebug('****Incomplete List')
self.VecNeural_z_previous = neuralz_current
else:
diff = abs(neuralz_current - self.VecNeural_z_previous)
# rospy.logdebug('------------------------------')
# rospy.logdebug('Diff points: %f',diff)
if diff > 0.4 and self.VecNeural_z_previous != 0:
self.VecNeural_z_previous = neuralz_current
# self.VecNeural.z = 0
# rospy.logdebug('------------------------------')
# rospy.logdebug('****No capture!')
else:
self.list_z.append(neuralz_current)
del self.list_z[0]
neuralz_current = sum(self.list_z) / len(self.list_z)
self.VecNeural.z = neuralz_current
# rospy.logdebug('------------------------------')
# rospy.logdebug('Size of list_z: %f',len(self.list_z))
# rospy.logdebug('Sum List list_z: %f',sum(self.list_z))
# rospy.logdebug('Med List list_z: %f',self.VecNeural.z)
self.VecNeural_z_previous = neuralz_current
self.rcnn_odom.header.stamp = rospy.Time.now()
self.rcnn_odom.header.seq = self.Keyframe_rcnn
# stabilize angles and align transformations
explicit_quat = [
self.OriAruco.x, self.OriAruco.y, self.OriAruco.z,
self.OriAruco.w
]
euler = tf.transformations.euler_from_quaternion(explicit_quat)
# roll = euler[0]
# pitch = euler[1]
yaw = euler[2]
# # since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(0, 0, -yaw)
# set the position
self.rcnn_odom.pose.pose = Pose(self.VecNeural,
Quaternion(*odom_quat))
tf_rcnn_to_drone = tf.TransformBroadcaster()
tf_rcnn_to_drone.sendTransform(
(self.VecNeural.x, self.VecNeural.y, self.VecNeural.z),
odom_quat, self.rcnn_odom.header.stamp, "rcnn_odom",
self.PARENT_NAME) #world
self.Keyframe_rcnn += 1
self.odom_rcnn_pub.publish(self.rcnn_odom)
def __init__(self, link_to_robot, tf_prefix):
self.broadcaster = tf.TransformBroadcaster()
self.listener = tf.TransformListener()
self.link_to_robot = link_to_robot
self.odom_data = Odometry()
self.tf_prefix = tf_prefix
def __init__(self):
super(Subscriber, self).__init__()
rospy.init_node('filter_node', anonymous=False, log_level=rospy.DEBUG)
self.PARENT_NAME = rospy.get_param('~parent_name', "camera_odom")
rospy.logdebug("%s is %s default %s",
rospy.resolve_name('~parent_name'), self.PARENT_NAME,
"camera_odom")
self.kalman = Kalman(n_states=3, n_sensors=6)
self.kalman.P *= 10
self.kalman.R *= 0.02
self.VecNeural = Vector3()
self.VecAruco = Vector3()
self.OriAruco = Quaternion(0, 0, 0, 1)
self.Keyframe_aruco = 0
self.Keyframe_rcnn = 0
self.Keyframe_hybrid = 0
self.aruco_odom = Odometry()
self.aruco_odom.header.stamp = rospy.Time.now()
self.aruco_odom.header.frame_id = "aruco_odom"
self.aruco_odom.header.seq = self.Keyframe_aruco
self.aruco_odom.child_frame_id = self.PARENT_NAME
self.rcnn_odom = Odometry()
self.rcnn_odom.header.stamp = rospy.Time.now()
self.rcnn_odom.header.frame_id = "rcnn_odom"
self.rcnn_odom.header.seq = self.Keyframe_rcnn
self.rcnn_odom.child_frame_id = self.PARENT_NAME
self.list_x = []
self.list_y = []
self.list_z = []
self.VecNeural_x_previous = 0
self.VecNeural_y_previous = 0
self.VecNeural_z_previous = 0
self.list_kalma_x = []
self.list_kalma_y = []
self.list_kalma_z = []
self.filtered = Vector3()
# Publishers
self.pub_hybrid = rospy.Publisher('kalman/hybrid_raw',
Vector3,
queue_size=1)
self.pub_hybrid_filtred = rospy.Publisher('kalman/hybrid_filtred',
Vector3,
queue_size=1)
self.odom_filter_pub = rospy.Publisher("odom_filter",
Odometry,
queue_size=1)
self.odom_rcnn_pub = rospy.Publisher("odom_rcnn",
Odometry,
queue_size=1)
self.odom_aruco_pub = rospy.Publisher("odom_aruco",
Odometry,
queue_size=1)
rospy.Subscriber("rcnn/objects",
Detection2DArray,
self.callbackPoseRCNN,
queue_size=1)
rospy.Subscriber("aruco_double/pose",
Pose,
self.callbackPoseAruco,
queue_size=1)
r = rospy.Rate(60.0)
while not rospy.is_shutdown():
neural = [self.VecNeural.x, self.VecNeural.y, self.VecNeural.z]
aruco = [self.VecAruco.x, self.VecAruco.y, self.VecAruco.z]
mat = np.matrix(np.concatenate((neural, aruco), axis=None)).getT()
if self.kalman.first:
# insert the first 3 values of the vector
self.kalman.x = mat[0:3, :]
self.kalman.first = False
current_time = rospy.Time.now()
dt_aruco = (current_time - self.aruco_odom.header.stamp).to_sec()
dt_rcnn = (current_time - self.rcnn_odom.header.stamp).to_sec()
# module_rcnn = math.sqrt(abs(mat[3]**2)+abs(mat[4]**2)+abs(mat[5]**2))
# module_aru = math.sqrt(abs(mat[0]**2)+abs(mat[1]**2)+abs(mat[2]**2))
# rospy.logdebug("------------------------")
# rospy.logdebug("module_aru : %f", module_aru)
# rospy.logdebug("module_rcnn : %f", module_rcnn)
##################################################################################
# rcnn = 0
if mat[2] == 0 or dt_rcnn > 5.0:
covNeural = 10
covAruco = 0.01 * abs(self.kalman.x[2]) + 1.0
# rospy.logdebug("*****Neural = 0 ou stoped!*****")
# aruco = 0
elif mat[5] == 0 or dt_aruco > 5.0:
covNeural = (1.5 / (abs(self.kalman.x[2]) + 0.1)) + 1.0
covAruco = 10
# rospy.logdebug("*****aruco = 0 ou stoped!*****")
else:
# greater neural error and lower aruco error at low height
covNeural = (0.05 / (abs(self.kalman.x[2]) + 0.3)) + 0.45
covAruco = 0.05 * abs(self.kalman.x[2]) + 0.4
# rospy.logdebug("*****all-run!*****")
##################################################################################
# module_aru_ant = module_aru
# module_rcnn_ant = module_rcnn
# set values of cov in R matrix
arrayNeral = np.full((1, 3), covNeural, dtype=float)
arrayAruco = np.full((1, 3), covAruco, dtype=float)
Zarray = np.concatenate((arrayNeral, arrayAruco), axis=None)
self.kalman.R = np.diag(Zarray)
# rospy.logdebug("arrayNeral : %f", covNeural)
# rospy.logdebug("arrayAruco : %f", covAruco)
# rospy.logdebug("------------------------")
self.kalman.predict()
self.kalman.update(mat)
vec = Vector3()
vec.x = self.kalman.x[0]
vec.y = self.kalman.x[1]
vec.z = self.kalman.x[2]
# rospy.logdebug("------------------------")
# rospy.logdebug("kalman.sensor[1].x : %f", vec.x)
# rospy.logdebug("kalman.sensor[1].y : %f", vec.y)
# rospy.logdebug("kalman.sensor[1].z : %f", vec.z)
size_filter_kalman_high = 20
# Filter list_kalma_x
if len(self.list_kalma_x) < size_filter_kalman_high:
self.list_kalma_x.append(vec.x)
else:
mean_filter = sum(self.list_kalma_x) / len(self.list_kalma_x)
if abs(mean_filter - vec.x) > 0.05:
self.list_kalma_x.append(vec.x)
del self.list_kalma_x[0]
self.filtered.x = sum(self.list_kalma_x) / len(
self.list_kalma_x)
# Filter list_kalma_y
if len(self.list_kalma_y) < size_filter_kalman_high:
self.list_kalma_y.append(vec.y)
else:
mean_filter = sum(self.list_kalma_y) / len(self.list_kalma_y)
if abs(mean_filter - vec.y) > 0.05:
self.list_kalma_y.append(vec.y)
del self.list_kalma_y[0]
self.filtered.y = sum(self.list_kalma_y) / len(
self.list_kalma_y)
# Filter list_kalma_z
if len(self.list_kalma_z) < size_filter_kalman_high:
self.list_kalma_z.append(vec.y)
else:
mean_filter = sum(self.list_kalma_z) / len(self.list_kalma_z)
if abs(mean_filter - vec.z) > 0.05:
self.list_kalma_z.append(vec.z)
del self.list_kalma_z[0]
self.filtered.z = sum(self.list_kalma_z) / len(
self.list_kalma_z)
##################################################################################
if dt_aruco < 0.1 or dt_rcnn < 0.1:
hybrid_odom = Odometry()
hybrid_odom.header.stamp = rospy.Time.now()
hybrid_odom.header.frame_id = "hybrid_odom"
hybrid_odom.header.seq = self.Keyframe_hybrid
hybrid_odom.child_frame_id = self.PARENT_NAME
explicit_quat = [
self.OriAruco.x, self.OriAruco.y, self.OriAruco.z,
self.OriAruco.w
]
euler = tf.transformations.euler_from_quaternion(explicit_quat)
# roll = euler[0]
# pitch = euler[1]
yaw = euler[2]
# # since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, -yaw)
# set the position
hybrid_odom.pose.pose = Pose(self.filtered,
Quaternion(*odom_quat))
# transform tf
tf_hybrid_to_drone = tf.TransformBroadcaster()
tf_hybrid_to_drone.sendTransform(
(self.filtered.x, self.filtered.y, self.filtered.z),
odom_quat, hybrid_odom.header.stamp, "hybrid_odom",
self.PARENT_NAME) #world
##################################################################################
self.Keyframe_hybrid += 1
# publish the message
self.odom_filter_pub.publish(hybrid_odom)
self.pub_hybrid_filtred.publish(self.filtered)
# publish the hybrid raw
self.pub_hybrid.publish(vec)
r.sleep()
try:
rospy.spin()
except rospy.ROSInterruptException:
print("Shutting down")
def broadcast_transform(self, name, position, orientation):
br = tf.TransformBroadcaster()
br.sendTransform(position, orientation, rospy.Time.now(), name,
"world")
def detect_feature_callback(req):
# print "----------------------------------------------"
# print "detect_feature_callback: "
# print " camera frame_id [%s] :"%(req.cameraPose.header.frame_id)
# print " camera translation from map [%.4f,%.4f,%.4f] : "%(req.cameraPose.pose.position.x,req.cameraPose.pose.position.y,req.cameraPose.pose.position.z)
# print " camera orientation (quaternion) [%.4f,%.4f,%.4f,%.4f] : "%(req.cameraPose.pose.orientation.x,req.cameraPose.pose.orientation.y,req.cameraPose.pose.orientation.z,req.cameraPose.pose.orientation.w)
# print " feature algorithm [%s] : "%(req.visualFeature.algorithm)
# print " feature id [%d] : "%(req.visualFeature.id)
# print ' feature frame_id : %s_%s%s' % (req.cameraPose.header.frame_id, algorithm_abreviations[req.visualFeature.algorithm], req.visualFeature.id)
# print " feature translation [%.4f,%.4f,%.4f] : "%(req.visualFeature.pose.pose.position.x, req.visualFeature.pose.pose.position.y, req.visualFeature.pose.pose.position.z)
# print " feature orientation (quaternion) [%.4f,%.4f,%.4f,%.4f] "%(req.visualFeature.pose.pose.orientation.x, req.visualFeature.pose.pose.orientation.y, req.visualFeature.pose.pose.orientation.z, req.visualFeature.pose.pose.orientation.w)
cN = req.cameraPose.header.frame_id
if req.visualFeature.id not in features_present:
print "detect_feature_callback: camera frame_id [%s] : feature id [%d] : feature is not present - is a false positive - not listing as a detection." % (
req.cameraPose.header.frame_id, req.visualFeature.id)
response = DetectedFeatureResponse()
response.acknowledgement = "feature not present"
return response
# TODO - something about this conversion is not right: the translation and quaternion match those found by AprilTags_Kaess and sent by DetectedFeatureClient, but the RPY are different
euler = tf.transformations.euler_from_quaternion([
req.visualFeature.pose.pose.orientation.x,
req.visualFeature.pose.pose.orientation.y,
req.visualFeature.pose.pose.orientation.z,
req.visualFeature.pose.pose.orientation.w
])
print " feature x_y_z_w roll=%.4f pitch=%.4f yaw=%.4f" % (
euler[0], euler[1], euler[2])
tfBroadcaster = tf.TransformBroadcaster()
time_now = rospy.Time.now()
# publish the map-to-camera_pose tf
tfBroadcaster.sendTransform(
(req.cameraPose.pose.position.x, req.cameraPose.pose.position.y,
req.cameraPose.pose.position.z),
(req.cameraPose.pose.orientation.x, req.cameraPose.pose.orientation.y,
req.cameraPose.pose.orientation.z, req.cameraPose.pose.orientation.w),
time_now,
req.cameraPose.header.
frame_id, # to '/cam/%s/pose' % req.cameraPose.header.frame_id e.g. "/cam/c_1/pose" # TODO - remove hardcoding to base namespace
'map') # from frame
# publish the map-to-camera_pose tf
tfBroadcaster.sendTransform(
(req.cameraPose.pose.position.x, req.cameraPose.pose.position.y,
req.cameraPose.pose.position.z),
(req.cameraPose.pose.orientation.x, req.cameraPose.pose.orientation.y,
req.cameraPose.pose.orientation.z, req.cameraPose.pose.orientation.w),
time_now,
req.cameraPose.header.frame_id +
'_in_detect_feature_req_header', # to '/cam/%s/pose' % req.cameraPose.header.frame_id e.g. "/cam/c_1/pose" # TODO - remove hardcoding to base namespace
'map') # from frame
# create the robot-convention camera body frame, step 1 : +90Y
tfBroadcaster.sendTransform(
(0.0, 0.0, 0.0),
(
0.0, 0.7071, 0.0, 0.7071
), # http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/steps/index.htm
time_now,
req.cameraPose.header.frame_id + 'y', # to
req.cameraPose.header.frame_id) # from
# create the robot-convention camera body frame, step 2 : +90Z
tfBroadcaster.sendTransform(
(0.0, 0.0, 0.0),
(
0.0, 0.0, 0.7071, 0.7071
), # http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/steps/index.htm
time_now,
req.cameraPose.header.frame_id + 'yz', # to
req.cameraPose.header.frame_id + 'y') # from
camera_tag_frame_id = '%s_%s%s' % (
req.cameraPose.header.frame_id,
algorithm_abreviations[req.visualFeature.algorithm],
req.visualFeature.id)
t55 = '%s%s' % (algorithm_abreviations[req.visualFeature.algorithm],
req.visualFeature.id)
# THIS IS THE GOOD TAG POSITION, AND GOOD TAG ORIENTATION BUT THE TAG FACES AWAY FROM THE CAMERA
# INTIIALLY GOOD AND ALIGNED WITH c2_from_fixed_t55 BUT THEN ROTATES STRANGELY IF THE CAMERA ROTATES
t55_from_c2 = '%s%s_from_%s' % (
algorithm_abreviations[req.visualFeature.algorithm],
req.visualFeature.id, req.cameraPose.header.frame_id)
tfBroadcaster.sendTransform(
(req.visualFeature.pose.pose.position.x,
req.visualFeature.pose.pose.position.y,
req.visualFeature.pose.pose.position.z),
#(req.visualFeature.pose.pose.orientation.x, req.visualFeature.pose.pose.orientation.y, req.visualFeature.pose.pose.orientation.z, req.visualFeature.pose.pose.orientation.w),
(req.visualFeature.pose.pose.orientation.z,
-req.visualFeature.pose.pose.orientation.x,
-req.visualFeature.pose.pose.orientation.y,
req.visualFeature.pose.pose.orientation.w),
time_now,
t55_from_c2, # to tag-from-camera-body t55_from_c2
req.cameraPose.header.frame_id) # from camera-body c2
# GOOD: t55_from_c2_180x
# GOOD with translationRotationWithAxisChange --> tagDetection.getRelativeTranslationRotation
# Problem is the once-off localisation against the fixed tag
# THIS IS THE GOOD TAG POSE, with the visible tag facing toward the camera
# INTIIALLY GOOD BUT THEN ROTATES STRANGELY WITH t55_from_c2 IF THE CAMERA ROTATES
t55_from_c2_180x = t55_from_c2 + '_180x'
tfBroadcaster.sendTransform(
(0.0, 0.0, 0.0),
(1.0, 0.0, 0.0, 0.0),
time_now,
t55_from_c2_180x, # to
t55_from_c2) # from
# simplify the below (c2_from_t55_from_c2_180x is GOOD, but maybe simplify)
quat_t55_from_c2_plain = [
req.visualFeature.pose.pose.orientation.x,
req.visualFeature.pose.pose.orientation.y,
req.visualFeature.pose.pose.orientation.z,
req.visualFeature.pose.pose.orientation.w
]
quat_c2_from_t55_plain = inverse(quat_t55_from_c2_plain)
fixed_t55 = 'fixed_%s' % (t55)
# GOOD: c2_from_t55_from_c2_180x
# GOOD with translationRotationWithAxisChange --> tagDetection.getRelativeTranslationRotation
# Problem is the once-off localisation against the fixed tag
# NOTE 1: t55 from c2 is [ +z -x -y +w ]
# NOTE 2: c2 from t55 is inverse( [ +z -x -y +w ] )
quat_t55_from_c2 = [
req.visualFeature.pose.pose.orientation.z,
-req.visualFeature.pose.pose.orientation.x,
-req.visualFeature.pose.pose.orientation.y,
req.visualFeature.pose.pose.orientation.w
]
quat_c2_from_t55 = inverse(quat_t55_from_c2)
c2_from_t55_from_c2_180x = '%s_from_%s' % (cN, t55_from_c2_180x)
c2_from_t55_from_c2_180x_tmp = c2_from_t55_from_c2_180x + '_tmp'
tfBroadcaster.sendTransform(
#(req.visualFeature.pose.pose.position.x, req.visualFeature.pose.pose.position.y, -req.visualFeature.pose.pose.position.z),
(0.0, 0.0, 0.0),
quat_c2_from_t55,
time_now,
c2_from_t55_from_c2_180x_tmp, # to
t55_from_c2) # from
tfBroadcaster.sendTransform(
(-req.visualFeature.pose.pose.position.x,
-req.visualFeature.pose.pose.position.y,
-req.visualFeature.pose.pose.position.z),
(0.0, 0.0, 0.0, 1.0),
time_now,
c2_from_t55_from_c2_180x, # to
c2_from_t55_from_c2_180x_tmp) # from
# from fixed tag to camera
t55_mirrored = t55 + '_mirrored' # tag is rot+-180Z from robot pov: AprilTags gives tag pose as away from camera i.e. tag x-axis is into the tag / robot-convention frame aligned with back of tag, I treat tag x-axis as out of the tag / robot-convention axes aligned with face of tag
# t55 = '%s%s'%(algorithm_abreviations[req.visualFeature.algorithm], req.visualFeature.id)
c2_from_fixed_t55 = '%s_from_fixed_%s' % (cN, t55)
c2_from_fixed_t55_tmp = c2_from_fixed_t55 + '_tmp'
c2_from_fixed_t55_tmp180x = c2_from_fixed_t55 + '_tmp180x'
tfBroadcaster.sendTransform(
(
0.0, 0.0, 0.0
), # same position: _t55_tmp180x is the same position as t55 / t55 ...
(1.0, 0.0, 0.0, 0.0), # 180 around x: ... but rotated 180 around x
time_now, # simultaneous 'now'
c2_from_fixed_t55_tmp180x, # to
t55) # from
tfBroadcaster.sendTransform(
#(req.visualFeature.pose.pose.position.x, req.visualFeature.pose.pose.position.y, -req.visualFeature.pose.pose.position.z),
(0.0, 0.0, 0.0), # _tmp is same position as the fixed tag ...
quat_c2_from_t55, # ... but inverse quaternion to point back toward the camera
time_now, # simultaneous 'now'
c2_from_fixed_t55_tmp, # to
c2_from_fixed_t55_tmp180x) # from
tfBroadcaster.sendTransform(
(-req.visualFeature.pose.pose.position.x,
-req.visualFeature.pose.pose.position.y,
-req.visualFeature.pose.pose.position.z
), # inverse/reversed translation from fixed tag ...
(0.0, 0.0, 0.0, 1.0), # ... same orientation as the _tmp
time_now, # simultaneous 'now'
c2_from_fixed_t55, # to
c2_from_fixed_t55_tmp) # from
axisMarker(req.visualFeature.id, c2_from_fixed_t55)
tfBroadcaster.sendTransform(
(0, 0, 0), (req.visualFeature.pose.pose.orientation.x,
req.visualFeature.pose.pose.orientation.y,
req.visualFeature.pose.pose.orientation.z,
req.visualFeature.pose.pose.orientation.w), time_now,
camera_tag_frame_id, camera_tag_frame_id + 't'
) # from '/cam/%s/pose' % req.cameraPose.header.frame_id e.g. "/cam/c_1/pose" # TODO - remove hardcoding to base namespace
tag_label = '%s%s' % (algorithm_abreviations[req.visualFeature.algorithm],
req.visualFeature.id)
print camera_tag_frame_id + '_mirrored_translation', ': from camera to tag: x=', req.visualFeature.pose.pose.position.z, ', y=', -req.visualFeature.pose.pose.position.x, ', z=', -req.visualFeature.pose.pose.position.y
print camera_tag_frame_id + '_mirrored_translation', ': from tag to camera: x=', req.visualFeature.pose.pose.position.z, ', y=', -req.visualFeature.pose.pose.position.x, ', z=', req.visualFeature.pose.pose.position.y
for fixed_feature in fixed_features:
tfBroadcaster.sendTransform(
(fixed_feature.pose.position.x, fixed_feature.pose.position.y,
fixed_feature.pose.position.z),
(fixed_feature.pose.orientation.x,
fixed_feature.pose.orientation.y,
fixed_feature.pose.orientation.z,
fixed_feature.pose.orientation.w), time_now,
'%s%s' % (algorithm_abreviations[fixed_feature.algorithm],
fixed_feature.id), 'map')
tfBroadcaster.sendTransform(
(fixed_feature.pose.position.x, fixed_feature.pose.position.y,
fixed_feature.pose.position.z),
(fixed_feature.pose.orientation.x,
fixed_feature.pose.orientation.y,
fixed_feature.pose.orientation.z,
fixed_feature.pose.orientation.w), time_now,
'fixed_%s%s' % (algorithm_abreviations[fixed_feature.algorithm],
fixed_feature.id), 'map')
if 'c3' == cN:
try:
# LATER; 'c1_from_fixed_t32'
listener.waitForTransform(
'map', 't50_from_c1_180x', rospy.Time(), rospy.Duration(4.0)
) # from the map origin, to the camera, through a fixed point
except:
print 'ERROR ------ : no transform from %s to %s ' % (
'map', 't50_from_c1_180x')
response = DetectedFeatureResponse()
response.acknowledgement = "bob"
return response
