def __init__(self):
ns = "path_planner/"
self.goal = np.array(rospy.get_param(ns + 'goal', np.array([0., 0., np.pi/2]))) # What params file will the goal come from?
self.current_cell = np.array([0.0, 0.0, 0.0])
self.step = 4 # TODO: From voxel map params?
self.fov = 12 # TODO: From voxel map params?
self.map_width = self.fov*2 + 1
self.prob_map = np.ones((self.map_width, self.map_width, self.map_width))*0.01
self.planner = AStarSearch(start=self.current_cell, goal=self.goal,
prob_map=self.prob_map, fov=self.fov)
rospy.Subscriber("status", Status, self.statusCallback, queue_size=1)
self.counter = 0
self.armed = False
self.cell_sub = rospy.Subscriber("---------<TOPIC HERE>-----------", <---MESSAGE_TYPE--->, self.cellCallback) #TODO: Subscribe to the current cell position from voxel_map_node
self.cloud_sub = rospy.Subscriber("voxel_map", PointCloud, self.mapCallback)
self.wpt_pub = rospy.Publisher("waypoints", PoseArray, queue_size=1)
self.wpt_msg = PoseArray()
self.num_waypoints = 10 # TODO: From waypoints?
def faces_cb(self, msg):
# cropped_imgs = []
# centers_2d = []
# median_distances = []
pose_array = PoseArray()
pose_array.header.stamp = rospy.Time.now()
pose_array.header.frame_id = "interaction_rs_color_optical_frame"
for face in msg.faces:
if face.confidence < self.confidence_thr:
continue
pose = Pose()
pose.position = face.head_pose.position
pose.position.x /= 1000.
pose.position.y /= 1000.
pose.position.z /= 1000.
pose.orientation = face.head_pose.orientation
pose_array.poses.append(pose)
self.poseArray_pub.publish(pose_array)
def publish_information(self):
information = PoseArray()
for player in self.data_base.team['blue']:
player_pose = Pose()
ball_pose_gl = Pose()
ball_pose_lc = Pose()
ball_velo = Pose()
player_pose.position.x, player_pose.position.y, player_pose.position.z = player.get_pos()
ball_pose_gl.position.x, ball_pose_gl.position.y, _ = self.ball.get_pos(player.color)
ball_velo.position.x = (self.ball.pos[0] - self.ball.past_pos[0])*10.0
ball_velo.position.y = (self.ball.pos[1] - self.ball.past_pos[1])*10.0
ball_lc = geometry.coord_trans_global_to_local(player.get_pos(), self.ball.get_pos(player.color))
ball_pose_lc.position.x, ball_pose_lc.position.y, _ = ball_lc
information.poses = [player_pose, ball_pose_gl, ball_pose_lc, ball_velo]
if ball_velo.position.x !=0 or ball_velo.position.y !=0:
#print("engine:", ball_velo.position.x, ball_velo.position.y)
pass
self.pub.publish(information)
def _publishWaypoints(self, predict):
goal = []
msg = PoseArray()
header = std_msgs.msg.Header()
for pt in range(self.__model.num_points):
point = []
for coord in range(self.__model.outputs):
if not self.__regression:
bin_size = (self.__model.max[pt][coord] -
self.__model.min[pt][coord]) / float(
self.__model.bins)
point.append(self.__model.min[pt][coord] +
bin_size * predict[0, coord, pt])
else:
point.append(logits[0, pt, coord])
if self.__spherical:
pointList = [np.array(point)]
pl = sphericalToXYZ().__call__(pointList)
print("Before: ", pl.shape)
point = pl.flatten()
print("After: ", point.shape)
point = np.array(point)
#print(point)
goal.append(point)
pt_pose = Pose()
pt_pose.position.x = point[0]
pt_pose.position.y = point[1]
pt_pose.position.z = point[2]
R_quat = R.from_euler('ZYX', point[3:6]).as_quat()
pt_pose.orientation.x = R_quat[0]
pt_pose.orientation.y = R_quat[1]
pt_pose.orientation.z = R_quat[2]
pt_pose.orientation.w = R_quat[3]
msg.poses.append(pt_pose)
#exit()
header.stamp = rospy.Duration(self.waypoint_secs[0])
msg.header = header
self.__pub.publish(msg)
def pubhumans():
global pub3
global people
poses = PoseArray()
poses.header.frame_id = "map"
ps = []
flag = False
for person in people:
flag = True
if person.moving:
p = Pose()
p.position.x = float(person.x)
p.position.y = float(person.y)
p.orientation.x = 1
p.orientation.y = 0.001
p.orientation.z = 0
p.orientation.w = 0
if person.vx != 0 and person.vy != 0:
p.orientation = rotateQuaternion(p.orientation, math.atan2(person.vy, person.vx))
ps.append(p)
poses.poses = ps
pub3.publish(poses)
def __init__(self):
# ----- Initialise fields
self.estimatedpose = PoseWithCovarianceStamped()
self.occupancy_map = OccupancyGrid()
self.particlecloud = PoseArray()
self.tf_message = tfMessage()
self._update_lock = Lock()
# ----- Parameters
self.ODOM_ROTATION_NOISE = 0 # Odometry model rotation noise
self.ODOM_TRANSLATION_NOISE = 0 # Odometry x axis (forward) noise
self.ODOM_DRIFT_NOISE = 0 # Odometry y axis (side-side) noise
self.NUMBER_PREDICTED_READINGS = 20 # Number of readings to predict
# ----- Set 'previous' translation to origin
# ----- All Transforms are given relative to 0,0,0, not in absolute coords.
self.prev_odom_x = 0.0 # Previous odometry translation from origin
self.prev_odom_y = 0.0 # Previous odometry translation from origin
self.prev_odom_heading = 0.0 # Previous heading from odometry data
self.last_odom_pose = None
# ----- Request robot's initial odometry values to be recorded in prev_odom
self.odom_initialised = False
self.sensor_model_initialised = False
# ----- Set default initial pose to initial position and orientation.
self.estimatedpose.pose.pose.position.x = self.INIT_X
self.estimatedpose.pose.pose.position.y = self.INIT_Y
self.estimatedpose.pose.pose.position.z = self.INIT_Z
self.estimatedpose.pose.pose.orientation = rotateQuaternion(Quaternion(w=1.0),
self.INIT_HEADING)
# ----- NOTE: Currently not making use of covariance matrix
self.estimatedpose.header.frame_id = "/map"
self.particlecloud.header.frame_id = "/map"
# ----- Sensor model
self.sensor_model = sensor_model.SensorModel()
def make_msg(waypoints, ori, order):
wps = PoseArray()
print order
order2 = order[:-1]
for i in order2:
pose = Pose()
pose.position.x = waypoints[i][0]
pose.position.y = waypoints[i][1]
pose.position.z = waypoints[i][2]
pose.orientation.w = ori[i][0]
pose.orientation.x = ori[i][1]
pose.orientation.y = ori[i][2]
pose.orientation.z = ori[i][3]
wps.poses.append(pose)
return wps
def pipeline(self):
if self.img_receive: #and self.loaded_weights
#Run prediction (and optional, visualization)
self.seg_arr, self.image, self.output_image, self.fit = lane_predict.predict_on_image(
self.model, self.image, self.lane_fit, self.evaluate,
self.visualize, self.output_file, self.display)
final_img = self.visualize_functions()
#print len(fit)
if self.fit != None:
fit_points = PoseArray()
fit_points.header.frame_id = "camera_color_optical_frame"
fit_points.header.stamp = rospy.Time.now()
for i in range(0, len(self.fit), 4): # Increment by 4
P = Pose()
P.position.x = self.fit[i][0]
P.position.y = self.fit[i][1]
fit_points.poses.append(P)
self.fit_pub.publish(fit_points)
self.rate.sleep()
def bounding_boxes_callback(self, msg):
#print("call back")
poses = PoseArray()
pose = Pose()
for i in range(len(msg.bounding_boxes)):
xmin = msg.bounding_boxes[i].xmin
xmax = msg.bounding_boxes[i].xmax
ymin = msg.bounding_boxes[i].ymin
ymax = msg.bounding_boxes[i].ymax
bounding_box_x = float(int((xmax - xmin) / 2 + xmin))
bounding_box_y = float(int((ymax - ymin) / 2 + ymin))
pose.position.x = bounding_box_x
pose.position.y = bounding_box_y
pose.position.z = msg.bounding_boxes[i].id
poses.poses.append(pose)
self.bounding_box_pos_pub.publish(poses)
def visualize(self):
self.state_lock.acquire()
# YOUR CODE HERE
expected_pose = self.expected_pose()
#1
self.publish_tf(expected_pose)
#2
lasermsg = self.sensor_model.last_laser
#lasermsg.header.frame_id = "laser"
self.pub_laser.publish(self.sensor_model.last_laser)
#3
exp = PoseStamped()
exp.header.stamp = rospy.Time.now()
exp.header.frame_id = "map"
q = tf.transformations.quaternion_from_euler(0, 0, expected_pose[2])
exp.pose = Pose(Point(expected_pose[0], expected_pose[1], 0),
Quaternion(*q))
self.pose_pub.publish(exp)
#4
import time
a = time.time()
vizparts = PoseArray()
vizparts.header.stamp = rospy.Time.now()
vizparts.header.frame_id = "map"
indices = range(self.particles.shape[0])
picked_indices = np.random.choice(indices, 100, True, self.weights)
#instead of len(self.particles)
for i in picked_indices:
p = self.particles[i]
q = tf.transformations.quaternion_from_euler(0, 0, p[2])
ps = Pose(Point(p[0], p[1], 0), Quaternion(*q))
vizparts.poses.append(ps)
self.particle_pub.publish(vizparts)
#print('Viz: ', time.time() - a)
self.state_lock.release()
def __init__(self):
#Creating our node,publisher and subscriber
rospy.init_node('turtlebot_controller', anonymous=True)
self.velocity_publisher = rospy.Publisher('/cmd_vel',
Twist,
queue_size=10)
goal_pose_subscriber = message_filters.Subscriber(
'/to_pose_array/leg_detector', PoseArray)
pose_subscriber = message_filters.Subscriber('/odom', Odometry)
ts = message_filters.ApproximateTimeSynchronizer(
[goal_pose_subscriber, pose_subscriber], 1, slop=0.05)
ts.registerCallback(self.callback)
pose = Odometry()
#print(pose)
self.pose = pose.pose.pose.position
goal_pose = PoseArray()
#print(goal_pose)
try:
self.goal_pose = goal_pose.poses[0].position
except IndexError:
pass
self.rate = rospy.Rate(1)
def visualise_trajectory(self, waypoints):
"""Publish PoseArray representing the trajectory
:waypoints: list of Waypoint obj
:returns: None
"""
pose_array = PoseArray()
pose_array.header.frame_id = self.frame
pose_array.header.stamp = rospy.Time.now()
poses = []
current_time = 0.0
delta_time = 0.1
x, y, theta = self.current_position
last_wp_time = 0.0
for self._vel_curve_handler.trajectory_index, wp in enumerate(
waypoints[1:]):
while current_time < wp.time:
# current position has to be given (0,0,0) because the cmd_vel
# are applied in robot's frame whereas the poses are published
# in global frame
x_vel, y_vel, theta_vel = self._vel_curve_handler.get_vel(
current_time - last_wp_time,
current_position=(x, y, theta))
# current_position=(0.0, 0.0, 0.0))
x += x_vel * math.cos(theta) * delta_time - y_vel * math.sin(
theta) * delta_time
y += x_vel * math.sin(theta) * delta_time + y_vel * math.cos(
theta) * delta_time
theta += theta_vel * delta_time
pose = Utils.get_pose_from_x_y_theta(x, y, theta)
poses.append(pose)
current_time += delta_time
# time.sleep(delta_time)
# pose_array.poses = poses
# self._trajectory_pub.publish(pose_array)
last_wp_time = wp.time
pose_array.poses = poses
self._trajectory_pub.publish(pose_array)
def add_pose_and_publish_array(self,
topic,
pos_vec,
rot_quat,
frame="/map_ned",
wxyz=False):
"""
:param topic: ROS topic to publish to.
:param pos_vec: 3-dimensional indexable position
:param rot_quat: wxyz quaternion, indexable
:param frame: frame_id to publish to. Default: /map_ned
:return:
"""
pose = self._make_pose_msg(pos_vec, rot_quat, wxyz=wxyz)
if topic not in self.pose_arrays:
pa = PoseArray()
pa.header.frame_id = frame
path = Path()
path.header.frame_id = frame
self.pose_arrays[topic] = pa
self.paths[topic] = path
else:
pa = self.pose_arrays[topic]
path = self.paths[topic]
pa.header.stamp = rospy.get_rostime()
path.header.stamp = pa.header.stamp
pa.poses.append(pose)
ps = PoseStamped()
ps.header = pa.header
ps.pose = pose
path.poses.append(ps)
self._get_pub(topic).publish(pa)
self._get_pub(topic + "_path").publish(path)
def getDetections(self, req):
print('Request for detection received!! Waiting for detection updates...')
# To get the most recent detections , we reset all the previous poses and labels
self.poses = None
self.cls_ids = None
#self.panda_binImg = None #Reset this as well in order to get the binImg with latest panda-config in scene
if rospy.has_param('remove_panda'): # This param helps other nodes, signal this server whether to remove the scene robot or not.
self.remove_panda_dpt = rospy.get_param('remove_panda')
if rospy.has_param('pvn3d_cam'):
self.cam = rospy.get_param('pvn3d_cam') # Set the cam, we're getting detections from.
self.cam_frame = rospy.get_param('pvn3d_cam_frame') # Set camFrame, to publish the poses in.
# NOTE: In the final tutorial, we continuously switch between the camera fixed on robot and the camera looking at the scene & robot
# If the testing scenario has one camera only, this param shouldn't exist and the img topics should be the default ones in class def.
image_sub = message_filters.Subscriber(str(self.cam)+'/color/image_raw', Image, queue_size=1)
depth_sub = message_filters.Subscriber(str(self.cam)+'/depth/image_raw', Image, queue_size=1)
self.ts = message_filters.TimeSynchronizer([image_sub, depth_sub], 10)
self.ts.registerCallback(self.ros2torch_CB)
rospy.sleep(1)
# Finally set this, in order to get the TSCallback working.
self.request = True
while True: ## Wait for the time synchronizer to completely go through the ros2torch callback - This is roughly equal to the inference speed
if self.cls_ids is not None: #Whenever we get any detection updates...
self.request = False ## Reset the detection request so that the callback doesn't go in auto-detect mode.
self.panda_binImg = None #Reset this as well in order to get the binImg with latest panda-config in scene
poses_msg = []
for pose in self.poses:
rot = R.from_dcm(pose[0:3,0:3])
quat = rot.as_quat()
poses_msg.append( Pose(position = Point(x=pose[0,3],y=pose[1,3],z=pose[2,3]), orientation = Quaternion(x=quat[0],y=quat[1],z=quat[2],w=quat[3]) ) )
pose_arr_msg = PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.cam_frame), poses = poses_msg)
return getPoses_resp(pose_arr_msg , Floats( self.cls_ids.tolist() ))
def __init__(self):
rospy.init_node('move_car_client')
self.goal = MoveBaseGoal()
self.next_goal = MoveBaseGoal()
self.goal_sent = False
self.goal_cnt = 0
self.goals = []
self.waypoints = PoseArray()
self.waypoints.header.frame_id = "map"
# Creates the SimpleActionClient, passing the type of the action (MoveBaseAction) to the constructor.
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
wait = self.client.wait_for_server()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
return
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting sending goals...")
goal_sub = rospy.Subscriber('/simple_navigation_goal', PoseStamped,
self.pose_cb)
plan_sub = rospy.Subscriber('/move_base/GlobalPlanner/plan', Path,
self.plan_cb)
self.waypoints_pub = rospy.Publisher('/waypoint_array',
PoseArray,
queue_size=1)
self.plan_pub = rospy.Publisher('/full_plan', Path, queue_size=1)
self.full_plan = Path()
self.full_plan.header.frame_id = "map"
self.plan_pub.publish(self.full_plan)
def generate_viewpoints(self):
"""
Given a set of random viewpoints in a roi, filter those too close, and define a yaw for each.
1. needs to be further away than the lower threshold
2. must be in the same roi as WP
3. create yaw of robot view
N/A. as a backup, calculate the viewpoint from the waypoint
"""
view_goals = PoseArray()
view_goals.header.frame_id = "/map"
poses, yaws, dists = [], [], []
obj = self.selected_object_pose
for cnt, p in enumerate(self.possible_nav_goals.goals.poses):
x_dist = p.position.x - obj.position.x
y_dist = p.position.y - obj.position.y
# print "xDist %s, yDist %s" %(x_dist, y_dist)
dist = abs(math.hypot(x_dist, y_dist))
if dist > self.view_dist_thresh_low:
if self.robot_polygon.contains(
Point([p.position.x, p.position.y])):
# yaw = math.atan2(y_dist, x_dist)
p = self.add_quarternion(p, x_dist, y_dist)
poses.append(p)
# yaws.append(yaw)
# dists.append( (x_dist, y_dist) )
view_goals.poses = poses
self.pub_all_views.publish(view_goals)
self.possible_poses = poses
# self.possible_yaws = yaws
# add the viewpoint from the waypoint - as a back up if all others fail
x_dist = self.robot_pose.position.x - obj.position.x
y_dist = self.robot_pose.position.y - obj.position.y
dist = abs(math.hypot(x_dist, y_dist))
self.robot_pose = self.add_quarternion(self.robot_pose, x_dist, y_dist)
def gen_random_particles(self, number_of_particles):
particles = PoseArray()
map_width = self.occupancy_map.info.width
map_height = self.occupancy_map.info.height
accepted_particles = 0
while accepted_particles < number_of_particles:
x = random.randint(0, map_width - 1)
y = random.randint(0, map_height - 1)
theta = random.uniform(0, 2 * math.pi)
pose = Pose()
pose.position.x = x * 0.05
pose.position.y = y * 0.05
pose.orientation = rotateQuaternion(Quaternion(w=1.0), theta)
map_index = x + y * map_width
if self.occupancy_map.data[map_index] == 0:
particles.poses.append(pose)
accepted_particles += 1
return particles
def send_balles(self):
msg = PoseArray()
x_offset = 30
y_offset = 683
scale = 0.02481389578163772
for i in range(len(self.array_balles.poses)):
if (self.array_balles.poses[i].position.z == 1.):
pose = Pose()
pose.position.x = (self.array_balles.poses[i].position.x -
x_offset) * scale
pose.position.y = (-self.array_balles.poses[i].position.y +
y_offset) * scale
pose.position.z = self.array_balles.poses[i].position.z
msg.poses.append(pose)
while (len(msg.poses) < 10):
pose = Pose()
pose.position.x = 0.
pose.position.y = 0.
pose.position.z = 0.
msg.poses.append(pose)
self.publisher_pose.publish(msg)
def input_Thread():
print("Input Thread Started")
while not rospy.is_shutdown():
filename = input("Enter desired trajectory name: ") + ".csv"
rospy.wait_for_service('get_trajectory_CSV')
try:
CSV_service = rospy.ServiceProxy('get_trajectory_CSV', readCSV)
response = CSV_service(filename)
waypoints = response.waypoints
except:
print("Service failed, likely invalid trajectory")
if waypoints == PoseArray():
print("Null array returned, likely invalid file name")
else:
print("Trajectory sent")
waypoint_pub.publish(waypoints)
r.sleep()
def gotovo(nodes):
final_path=Marker()
final_path.type=4
final_path.header.frame_id='odom'
final_path.scale.x=0.3
final_path.color.g=1.0
final_path.color.a=1.0
pose_array=PoseArray()
for i in xrange(0,len(nodes)):
path_point=Point()
path_point_pose=Pose()
path_point_pose.position.x=nodes[i][0]
path_point_pose.position.y=nodes[i][1]
path_point.x=nodes[i][0]
path_point.y=nodes[i][1]
final_path.points.append(path_point)
pose_array.poses.append(path_point_pose)
final_path_pub.publish(final_path)
send_pose_array.publish(pose_array)
rate.sleep()
def recorded_path(self,repeat_no):
global pub
rate = rospy.Rate(1) # 1hz
rate.sleep()
count = 0
print("start generate path")
while not rospy.is_shutdown() and count < repeat_no:
if self.check_reaching_last_target(self.constant_path) == True or self.starting == 1:
self.starting = 0
pub_data = PoseArray()
for i in self.constant_path:
pose = Pose()
pose.position.x = i[0]
pose.position.y = i[1]
pose.orientation.w = i[2]
pub_data.poses.append(pose)
pub.publish(pub_data)
count = count + 1
print("generate no: "+str(count))
print("end generate route")
break;
rate.sleep()
print("generate path end")
def metaclustering(clusters):
cluster_poses = []
for cluster in clusters:
pose = Pose()
pose.position.x = cluster[0]
pose.position.y = cluster[1]
cluster_poses.append(pose)
pose_arr = PoseArray()
pose_arr.poses = cluster_poses
rospy.wait_for_service('/metaclustering')
try:
master_pose_rec_service = rospy.ServiceProxy('/metaclustering',
ClusterLocs)
runtime = []
runtime.append([get_size(pose_arr) * len(pose_arr.poses), 0])
with open(
"/DataStorage/clustering_runtime_send" +
rospy.get_namespace().replace("/", '') + ".csv", "wb") as f:
writer = csv.writer(f)
writer.writerows(runtime)
master_pose_rec_service(pose_arr)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def publish_particles(self, data):
particles_conv = []
for p in data.pose:
particles_conv.append(p)
# actually send the message so that we can view it in rviz
self.particle_pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.ns + 'base_link'),
poses=particles_conv))
marker_array = []
for index, i in enumerate(data.pose):
marker = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.ns + 'base_link'),
pose=i,
type=0,
scale=Vector3(x=i.orientation.w * 2,
y=i.orientation.w * 1,
z=i.orientation.w * 5),
id=index)
marker_array.append(marker)
self.marker_pub.publish(MarkerArray(markers=marker_array))
def __init__(self, robot):
Task2State.__init__(
self,
robot,
outcomes=['succeeded', 'failed'],
input_keys=['orig_global_trans', 'search_count', 'search_failed'],
output_keys=['orig_global_trans', 'search_count', 'search_failed'])
self.skip_look_down = rospy.get_param('~skip_look_down', False)
self.no_lookdown_mode = rospy.get_param('~no_lookdown_mode')
self.do_object_recognition = rospy.get_param('~do_object_recognition')
self.object_approach_height = rospy.get_param(
'~object_approach_height')
self.object_yaw_thresh = rospy.get_param('~object_yaw_thresh')
self.grasping_yaw = rospy.get_param('~grasping_yaw')
self.recognition_wait = rospy.get_param('~recognition_wait')
self.global_lookdown_pos_gps = rospy.get_param(
'~global_lookdown_pos_gps')
self.object_pose_sub = rospy.Subscriber(
'rectangle_detection_color/target_object_color', PoseArray,
self.objectPoseCallback)
self.object_pose = PoseArray()
def init_RCs(self):
self.current_roller_container_0 = Pose()
self.current_roller_container_0.position.x = 0.5525
self.current_roller_container_0.position.y = 1.4
# self.current_roller_container_0.position.x = 0.4-0.112
# self.current_roller_container_0.position.y = 1.4
# self.current_roller_container_1 = copy.deepcopy(self.current_roller_container_0)
# self.current_roller_container_1.position.x += self.rc_base_width + 0.05
# self.current_roller_container_2 = copy.deepcopy(self.current_roller_container_1)
# self.current_roller_container_2.position.x += self.rc_base_width + 0.05
# self.current_roller_container_3 = copy.deepcopy(self.current_roller_container_2)
# self.current_roller_container_3.position.x += self.rc_base_width + 0.05
self.current_roller_containers = PoseArray()
# self.current_roller_containers.poses = [self.current_roller_container_0, self.current_roller_container_1, self.current_roller_container_2, self.current_roller_container_3]
self.current_roller_containers.poses = [
self.current_roller_container_0
]
roller_containers = AddRCs()
roller_containers.rcs = self.current_roller_containers
added_rcs = self.add_rc(roller_containers)
def newPoint(pose):
print("Recieved point")
global pub
arr = PoseArray()
arr.header.frame_id = odomTf
robotPos = Pose()
t = tfListener.getLatestCommonTime(robotTf, odomTf)
position, rotation = tfListener.lookupTransform(odomTf, robotTf, t)
robotPos.position.x = position[0]
robotPos.position.y = position[1]
robotPos.position.z = 0.0
robotPos.orientation.x = rotation[0]
robotPos.orientation.y = rotation[1]
robotPos.orientation.z = rotation[2]
robotPos.orientation.w = rotation[3]
arr.poses.append(robotPos)
arr.poses.append(pose.pose)
pub.publish(arr)
def get_eef_poses_from_trajectory(self, trajectory):
"""
For a RobotTrajectory, determine the end-effector pose at each
time step along the trajectory.
TODO: test this function
"""
# Create the output array
poses_out = PoseArray()
poses_out.header.seq = copy.deepcopy(
trajectory.joint_trajectory.header.seq)
poses_out.header.stamp = rospy.Time.now()
poses_out.header.frame_id = 1
# Loop setup
poses_tot = len(trajectory.joint_trajectory.points)
pose_list = [None] * poses_tot
joint_names = trajectory.joint_trajectory.joint_names
# Build array of 'PoseStamped' waypoints
for i in range(0, poses_tot):
# Get the pose using FK
joint_states = trajectory.joint_trajectory.points[i].positions
time = trajectory.joint_trajectory.points[i].time_from_start
resp = self.fk_solve(joint_states, joint_names)
# Put the pose into list
pose.header.seq = i
pose.header.stamp = time
pose_list[i] = resp.pose_stamped
# Put generated list into the PoseArray
poses_out.poses = pose_list
return poses_out
def odom_callback(self, odom_msg):
self.publishTrue()
landmark_arr = PoseArray()
x = odom_msg.pose.pose.position.x
y = odom_msg.pose.pose.position.y
z = odom_msg.pose.pose.position.z
robot_pose = np.array([[x, y, z]])
dists = np.sqrt(np.sum((self.true_poses - robot_pose)**2, axis=1))
for i in range(0, len(dists)):
if (dists[i] <= self.threshold and not self.is_noise):
landmark_arr.poses.append(Pose())
landmark_arr.poses[len(landmark_arr.poses) -
1].position.x = self.true_poses[i, 0]
landmark_arr.poses[len(landmark_arr.poses) -
1].position.y = self.true_poses[i, 1]
landmark_arr.poses[len(landmark_arr.poses) -
1].orientation.w = i
elif (dists[i] <= self.threshold and self.is_noise):
landmark_arr.poses.append(Pose())
landmark_arr.poses[len(landmark_arr.poses) -
1].position.x = self.true_poses[
i, 0] + np.random.normal(0, 0.1)
landmark_arr.poses[len(landmark_arr.poses) -
1].position.y = self.true_poses[
i, 1] + np.random.normal(0, 0.1)
landmark_arr.poses[len(landmark_arr.poses) -
1].orientation.w = i
landmark_arr.header.stamp = rospy.Time.now()
landmark_arr.header.frame_id = '/map'
self.pub_landmarks.publish(landmark_arr)
def __init__(self, poseBuffSize):
##################################
# Give parameters in deg, meters #
##################################
self.poseBuffSize = poseBuffSize
##################################
# ## # # # # # # # # # # # # # # #
##################################
# Instantiate CvBridge
self.bridge = CvBridge()
# Init Listener for tf-transformation
self.tfListener = tf.TransformListener()
self.tfBroadcaster = tf.TransformBroadcaster()
# Init variables
self.markerPoses = PoseArray()
self.meanPose = Pose()
self.poseBuff = collections.deque(maxlen=self.poseBuffSize)
self.idx = 0
objName = rospy.get_param("object_name")
self.transformToPoints = rospy.get_param(str(objName) + "/transformTo")
self.ur5 = ur5_control.ur5Controler("camera_planning_frame",
"/mean_marker_pose", True)
rospy.sleep(1) # Wait to register at tf
# Subscriber to Object-Pose
rospy.Subscriber("/fiducial_transforms",
FiducialTransformArray,
self.marker_pose_callback,
queue_size=1) # Marker-Pose
rospy.Subscriber("/fiducial_images", Image, self.image_callback)
# Publisher for Pose-Array
self.pub = rospy.Publisher('/tf_meanMarkerPose', Pose, queue_size=10)
def parse_posearray(posearray_input):
"""
Parse posearray_input into a PoseArray.
"""
try:
assert isinstance(posearray_input, PoseArray)
return posearray_input
except:
pass
try:
assert isinstance(posearray_input, list)
posearray = PoseArray()
for pose in posearray_input:
try:
assert isinstance(pose, Pose)
posearray.poses.append(pose)
continue
except:
pass
try:
assert isinstance(pose, PoseStamped)
posearray.poses.append(pose.pose)
continue
except:
pass
try:
position = Point(x=pose[0][0], y=pose[0][1], z=pose[0][2])
orientation = Quaternion(x=pose[1][0], y=pose[1][1], z=pose[1][2], w=pose[1][3])
pose = Pose(position=position, orientation=orientation)
posearray.poses.append(pose)
continue
except Exception as e:
raise ValueError('Pose in pose array input not properly specified (should be Pose, PoseStamped or [[3],[4]] list)!')
posearray.header.stamp = rospy.Time.now()
return posearray
except Exception as e:
raise ValueError('Pose array not properly specified (should be PoseArray or list of Pose, PoseStamped or [[3],[4]] list types)!')
