def calculate_center_of_mass(self):
x = 0
y = 0
z = 0
for link in self.links:
# print (self.links[link]) # get structure of link
try:
# get transform of each link with respect to base link
self.tf_base_to_link = self.tf_buffer.lookup_transform(
"base_link", link, rospy.Time(), rospy.Duration(
1.0)) # 1 second timeout, blocks until it finds it
self.link_origin.point.x = self.links[
link].inertial.origin.xyz[0]
self.link_origin.point.y = self.links[
link].inertial.origin.xyz[1]
self.link_origin.point.z = self.links[
link].inertial.origin.xyz[2]
self.link_origin.header.frame_id = link
self.link_origin.header.stamp = rospy.get_rostime()
tf_base_to_link_origin = tf_msg.do_transform_point(
self.link_origin, self.tf_base_to_link)
# calculate part of CoM equation depending on link
x += self.links[link].inertial.mass * \
tf_base_to_link_origin.point.x
y += self.links[link].inertial.mass * \
tf_base_to_link_origin.point.y
z += self.links[link].inertial.mass * \
tf_base_to_link_origin.point.z
except tf2_ros.TransformException as err:
rospy.logerr(
"Calculate center of mass. TF error in COM computation %s",
err)
# finish CoM calculation
self.center_of_mass.header.stamp = rospy.Time.now()
self.center_of_mass.header.frame_id = "base_link"
self.center_of_mass.point.x = x / self.mass
self.center_of_mass.point.y = y / self.mass
self.center_of_mass.point.z = z / self.mass
self.pub_com.publish(self.center_of_mass)
# project COM into ground, z=0
self.tf_base_to_world = self.tf_buffer.lookup_transform(
"world", "base_link", rospy.Time(),
rospy.Duration(1.0)) # 1 second timeout, blocks until it finds it
self.tf_com_location_to_world = tf_msg.do_transform_point(
self.center_of_mass, self.tf_base_to_world)
self.center_of_mass_projected.header.frame_id = "world"
self.center_of_mass_projected.header.stamp = rospy.Time.now()
self.center_of_mass_projected.point.x = self.tf_com_location_to_world.point.x
self.center_of_mass_projected.point.y = self.tf_com_location_to_world.point.y
self.center_of_mass_projected.point.z = 0 # zero height
self.pub_projected.publish(self.center_of_mass_projected)
def get_obj_frames(blue_obj, red_obj, base_tf, base2kinect_tf):
base_pose = Pose()
base_pose.position = base_tf.transform.translation
base_pose.orientation = base_tf.transform.rotation
base_kdl = tf_conversions.fromMsg(base_pose)
base_unitX = base_kdl.M.UnitX()
base_unitY = base_kdl.M.UnitY()
base_unitZ = base_kdl.M.UnitZ()
### Frame for Blue Object
blue_center_kinect = PointStamped()
blue_center_kinect.header = base2kinect_tf.header
blue_center_kinect.point = blue_obj.bb_center
blue_center = tf2_geometry_msgs.do_transform_point(blue_center_kinect,
base2kinect_tf)
blue_pose = Pose()
blue_pose.position = blue_center.point
blue_pose.position.z = blue_pose.position.z - blue_obj.bb_dims.z / 2
blue_pose.orientation = base_tf.transform.rotation
blue_kdl = tf_conversions.fromMsg(blue_pose)
blue_pos = blue_kdl.p
blue_rot = PyKDL.Rotation(-base_unitX, -base_unitY, base_unitZ)
blue_kdl = PyKDL.Frame(blue_rot, blue_pos)
blue_pose = tf_conversions.toMsg(blue_kdl)
blue_frame = TransformStamped()
blue_frame.header.frame_id = base_frame
blue_frame.header.stamp = rospy.Time.now()
blue_frame.child_frame_id = 'blue_frame'
blue_frame.transform.translation = blue_pose.position
blue_frame.transform.rotation = blue_pose.orientation
### Frame for Red Object
red_center_kinect = PointStamped()
red_center_kinect.header = base2kinect_tf.header
red_center_kinect.point = red_obj.bb_center
red_center = tf2_geometry_msgs.do_transform_point(red_center_kinect,
base2kinect_tf)
red_pose = Pose()
red_pose.position = red_center.point
red_pose.position.z = red_pose.position.z - red_obj.bb_dims.z / 2
red_pose.orientation = base_tf.transform.rotation
red_kdl = tf_conversions.fromMsg(red_pose)
red_pos = red_kdl.p
red_rot = PyKDL.Rotation(-base_unitX, -base_unitY, base_unitZ)
red_kdl = PyKDL.Frame(red_rot, red_pos)
red_pose = tf_conversions.toMsg(red_kdl)
red_frame = TransformStamped()
red_frame.header.frame_id = base_frame
red_frame.header.stamp = rospy.Time.now()
red_frame.child_frame_id = 'red_frame'
red_frame.transform.translation = red_pose.position
red_frame.transform.rotation = red_pose.orientation
return blue_frame, red_frame
def get_next_setpoint(path):
global tf_buffer, robot_frame_id, look_ahead_distance, distance_converged
distance_left = look_ahead_distance
transform = tf_buffer.lookup_transform(robot_frame_id,
path.header.frame_id, rospy.Time(0),
rospy.Duration(1))
transform_inverse = tf_buffer.lookup_transform(path.header.frame_id,
robot_frame_id,
rospy.Time(0),
rospy.Duration(1))
point = tf2_geometry_msgs.do_transform_point(
PointStamped(path.header, path.poses[0].pose.position), transform)
distance = np.linalg.norm(np.array([[point.point.x, point.point.y]]))
last_point = PointStamped()
# print(len(path.poses))
if 1 == len(path.poses):
point.header = path.header
point.point = path.poses[0].pose.position
if distance < distance_converged:
del path.poses[0]
return point
elif distance <= (look_ahead_distance / 2.0):
del path.poses[0]
last_point = point
distance_left -= distance
elif distance > look_ahead_distance:
point.header = path.header
point.point = path.poses[0].pose.position
return point
last_position = np.array([[last_point.point.x, last_point.point.y]])
for pose in path.poses:
# FIXME: We assume that it is the same transform...
point = tf2_geometry_msgs.do_transform_point(
PointStamped(path.header, pose.pose.position), transform)
position = np.array([[point.point.x, point.point.y]])
distance = np.linalg.norm(position - last_position)
if distance > distance_left:
last_point.header = path.header
# Return interpolated between point and last_point
last_point.point = interpolate(last_point.point, point.point,
distance_left / distance)
return tf2_geometry_msgs.do_transform_point(
last_point, transform_inverse)
distance_left -= distance
last_point = point
last_position = position
point.header = path.header
point.point = path.poses[-1].pose.position
return point # Return last element
def move(path):
global control_client, robot_frame_id, pub
rate = rospy.Rate(10)
# Call service client with path
response = control_client(path)
# Transform Setpoint from service client
transform = tf_buffer.lookup_transform(robot_frame_id,
response.setpoint.header.frame_id,
rospy.Time(0))
transformed_setpoint = tf2_geometry_msgs.do_transform_point(
response.setpoint,
transform) #transform the setpoint to base_link frame
# Create Twist message from the transformed Setpoint
twist_msg = Twist()
twist_msg.angular.z = max_angular_velocity * atan2(
transformed_setpoint.point.y, transformed_setpoint.point.x)
twist_msg.linear.x = max_linear_velocity * sqrt(
transformed_setpoint.point.y**2 + transformed_setpoint.point.x**2)
if twist_msg.angular.z > max_angular_velocity:
twist_msg.angular.z = max_angular_velocity
if twist_msg.linear.x > max_linear_velocity:
twist_msg.linear.x = max_linear_velocity
# Publish Twist
pub.publish(twist_msg)
rate.sleep()
# Call service client again if the returned path is not empty and do stuff again
i = 0
while response.new_path.poses:
i = i + 1
print("There are poses " + str(i))
response = control_client(response.new_path)
transformer = tf_buffer.lookup_transform(
robot_frame_id, response.setpoint.header.frame_id, rospy.Time(0))
new_setpoint = tf2_geometry_msgs.do_transform_point(
response.setpoint,
transformer) #transform the setpoint to base_link frame
# Create Twist message from the transformed Setpoint
msg = Twist()
msg.angular.z = max_angular_velocity * atan2(
new_setpoint.point.y,
new_setpoint.point.x) ###########doesnt feel right
msg.linear.x = max_linear_velocity * sqrt(
new_setpoint.point.y**2 +
new_setpoint.point.x**2) ##############doesnt feel right
if msg.angular.z > max_angular_velocity:
msg.angular.z = max_angular_velocity
if msg.linear.x > max_linear_velocity:
msg.linear.x = max_linear_velocity
pub.publish(msg)
rate.sleep()
# Send 0 control Twist to stop robot
stop = Twist()
stop.angular.z = 0
stop.linear.x = 0
pub.publish(stop)
rate.sleep()
def move(path):
global control_client, robot_frame_id, pub
# Call service client with path
res = control_client(path)
# print("The frame is: %s" %res.setpoint.header.frame_id)
# Transform Setpoint from service client
transform = tf_buffer.lookup_transform(robot_frame_id, "map", rospy.Time())
transformed_setpoint = tf2_geometry_msgs.do_transform_point(res.setpoint, transform)
# Create Twist message from the transformed Setpoint
msg = Twist()
msg.angular.z = 4 * atan2(transformed_setpoint.point.y, transformed_setpoint.point.x)
if msg.angular.z > max_angular_velocity:
msg.angular.z = max_angular_velocity
msg.linear.x = 0.5 * math.sqrt(transformed_setpoint.point.x ** 2 + transformed_setpoint.point.y ** 2)
if msg.linear.x > max_linear_velocity:
msg.linear.x = max_linear_velocity
# Publish Twist
pub.publish(msg)
# Call service client again if the returned path is not empty and do stuff again
new_path = res.new_path
while new_path.poses:
res = control_client(new_path)
#tf_buffer = tf2_ros.Buffer() #necessary?
#listener = tf2_ros.TransformListener(tf_buffer) #necessary?
transform = tf_buffer.lookup_transform(robot_frame_id, "map", rospy.Time()) #necessary?
transformed_setpoint = tf2_geometry_msgs.do_transform_point(res.setpoint, transform)
msg = Twist() #necessary?
msg.angular.z = 4 * atan2(transformed_setpoint.point.y, transformed_setpoint.point.x)
if msg.angular.z > max_angular_velocity:
msg.angular.z = max_angular_velocity
msg.linear.x = 0.5 * math.sqrt(transformed_setpoint.point.x ** 2 + transformed_setpoint.point.y ** 2)
if msg.linear.x > max_linear_velocity:
msg.linear.x = max_linear_velocity
pub.publish(msg)
new_path = res.new_path
rate.sleep()
print("new_path.poses = " + str(new_path.poses))
# Send 0 control Twist to stop robot
msg.angular.z = 0
msg.linear.x = 0
pub.publish(msg)
def callback(input):
if is_similar(pose_old, input.pose.pose):
return
odom2base = TransformStamped()
odom2base.transform.translation = input.pose.pose.position
odom2base.transform.rotation = input.pose.pose.orientation
whl_l_pos = tf2_geometry_msgs.do_transform_point(base2wheel_l,
odom2base).point
traj["l"].append(whl_l_pos)
whl_r_pos = tf2_geometry_msgs.do_transform_point(base2wheel_r,
odom2base).point
traj["r"].append(whl_r_pos)
show_length()
def convert_to_worldframe(self, data):
try:
if data is None or self.location is None:
return
target_frame = "base_link"
source_frame = "camera_depth_optical_frame"
# define a source point
point_wrt_source = Point(data[0], data[1], data[2])
transformation = tf_buffer.lookup_transform(
target_frame, source_frame, rospy.Time(0), rospy.Duration(0.1))
point_wrt_target = tf2_geometry_msgs.do_transform_point(
PointStamped(point=point_wrt_source), transformation).point
self.publish_destination(
np.array([
self.location.x + point_wrt_target.x,
self.location.y + point_wrt_target.y,
self.location.z + point_wrt_target.z, point_wrt_target.x,
point_wrt_target.y, point_wrt_target.z
]))
except:
traceback.print_exc()
def transform_point(self, p, target_frame, source_frame, time=0.0):
""" Transform a point from the source frame to the target frame.
Parameters
----------
p : iterable, len 3
Input point in source frame.
target_frame : str
Name of the frame to transform into.
source_frame : str
Name of the input frame.
time : float, default 0.0
Time at which to get the transform. (0 will get the latest)
Returns
-------
tuple, len 3
Transformed point in target frame.
"""
import rospy
import tf2_geometry_msgs
from .msg import make_point_msg, unpack_point_msg
transform = self._buffer.lookup_transform(target_frame, source_frame,
rospy.Time.from_sec(time))
p_msg = make_point_msg(p, source_frame, time)
pt_msg = tf2_geometry_msgs.do_transform_point(p_msg, transform)
return unpack_point_msg(pt_msg, stamped=True)
def _range_bearing_callback(self, bearing_msg):
x = bearing_msg.range * \
m.cos(bearing_msg.bearing) * m.cos(bearing_msg.elevation)
y = bearing_msg.range * \
m.sin(bearing_msg.bearing) * m.cos(bearing_msg.elevation)
z = bearing_msg.range * m.sin(bearing_msg.elevation)
source_frame_point = PointStamped()
source_frame_point.header.stamp = rospy.Time.now()
source_frame_point.header.frame_id = bearing_msg.header.frame_id
source_frame_point.point.x = x
source_frame_point.point.y = y
source_frame_point.point.z = z
transform = self._tf_buffer.lookup_transform(target_frame=self._world_frame,
source_frame=source_frame_point.header.frame_id,
time=rospy.Time.now(),
timeout=rospy.Duration(
1.0)
)
target_frame_point = tf2_geometry_msgs.do_transform_point(
source_frame_point, transform)
rospy.loginfo("Located a beacon at %s" %
(" ".join(str(target_frame_point.point).split())))
rospy.loginfo(" - Distance : %7.2f" % (m.sqrt(x**2 + y**2)))
rospy.loginfo(" - Depth : %7.2f" % (z))
self._black_box_pose_pub.publish(target_frame_point)
self._publish_marker(target_frame_point)
def get_mask_img(self, transform, target_bin, camera_model):
"""
:param point: point that is going to be transformed
:type point: PointStamped
:param transform: camera_frame -> bbox_frame
:type transform: Transform
"""
# check frame_id of a point and transform just in case
assert camera_model.tf_frame == transform.header.frame_id
assert target_bin.bbox.header.frame_id == transform.child_frame_id
transformed_list = [
do_transform_point(corner, transform)
for corner in target_bin.corners]
projected_points = self.project_points(transformed_list, camera_model)
# generate an polygon that covers the region
path = Path(projected_points)
x, y = np.meshgrid(
np.arange(camera_model.width / self.camera_model.binning_x),
np.arange(camera_model.height / self.camera_model.binning_y))
x, y = x.flatten(), y.flatten()
points = np.vstack((x, y)).T
mask_img = path.contains_points(points).reshape(
camera_model.height / self.camera_model.binning_x,
camera_model.width / self.camera_model.binning_y)
mask_img = (mask_img * 255).astype(np.uint8)
return mask_img
def _publish_detection(self, detection_data, detection_frame,
detection_stamp):
source_detection_point = PointStamped()
source_detection_point.header.frame_id = detection_frame
source_detection_point.header.stamp = detection_stamp
source_detection_point.point = detection_data.location
transform = self._tf_buffer.lookup_transform(
target_frame=self._utm_frame,
source_frame=detection_frame,
time=rospy.Time.now(),
timeout=rospy.Duration(1.0))
utm_detection_point = tf2_geometry_msgs.do_transform_point(
source_detection_point, transform)
utm_pose = PoseStamped()
utm_pose.pose.orientation.x = 0.0
utm_pose.pose.orientation.y = 0.0
utm_pose.pose.orientation.z = 0.0
utm_pose.pose.orientation.w = 1.0
utm_pose.pose.position.x = utm_detection_point.point.x
utm_pose.pose.position.y = utm_detection_point.point.y
utm_pose.pose.position.z = utm_detection_point.point.z
geo_pose = utm_to_wgs84_pose(utm_pose, self._current_nav_sat_fix)
geo_pose.header.frame_id = detection_data.category
self._perception_pub.publish(geo_pose)
rospy.logwarn(geo_pose)
def callback(data):
#rospy.loginfo(rospy.get_caller_id() + "I heard %s", data.point)
# CoG_Position = data.point
# print("CoG Position: ", CoG_Position)
try:
trans = tfBuffer.lookup_transform("map", "base_link", rospy.Time(0),
rospy.Duration(0.01))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
raise
CoG_transformed = tf2_geometry_msgs.do_transform_point(
data, trans) # Transform Point from base_link frame to map frame
c = np.array([
CoG_transformed.point.x, CoG_transformed.point.y,
CoG_transformed.point.z
])
dcom, ac = com_sg.send(
(time.time(),
c)) # Applying SG filter to get velocity and acceleration
# contact forces (f / total_mass)
f = ac - gravity
# zram
alpha = (ground_level - c[2]) / f[2]
zram = c + alpha * f
array = Float32MultiArray(data=zram)
pub.publish(array)
def cb_object_detector(self, data):
if self.detector_state.is_initialized():
self.sensor_frame = data.header.frame_id
try:
# TF
self.trans_sensor_global = self.tf_buffer.lookup_transform(
self.global_frame, self.sensor_frame, rospy.Time())
detected_obstacles = []
for o in data.objects:
p = PointStamped()
p.point.x = o.pose.position.x
p.point.y = o.pose.position.y
p.point.z = o.pose.position.z
p_global = tf2_geometry_msgs.do_transform_point(
p, self.trans_sensor_global)
mind, mini = 0, np.inf
for i in range(
len(self.detector_state.global_waypoints.waypoints)
):
wp_position = self.detector_state.global_waypoints.waypoints[
i].pose.pose.position
dx = p_global.point.x - wp_position.x
dy = p_global.point.y - wp_position.y
d_eucl = np.sqrt(dx**2 + dy**2)
if (d_eucl < self.safety_threshold):
if d_eucl < mind:
mind = d_eucl
if mind < self.safety_threshold:
detected_obstacles.append(o)
print(len(detected_obstacles), len(data.objects))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logerr("TF error")
def _range_callback(self, msg):
with self._lock:
z = msg.pose.pose.position.z
_time = msg.header.stamp
if math.isinf(z) or z < self._range_min or z > self._range_max:
rospy.logerr("AltimeterFilter: rejecting range of {}".format(z))
return
try:
transform = self.tf_buffer.lookup_transform(
"base_link", msg.header.frame_id, _time, rospy.Duration(1.0)
)
except Exception as e:
rospy.logerr(
"AltimeterFilter: error looking up transform {}".format(e)
)
raise
stamped_point = PointStamped()
stamped_point.point.z = z
final_pose = tf2_geometry_msgs.do_transform_point(
stamped_point, transform
)
final_msg = PoseWithCovarianceStamped()
final_msg.header.frame_id = "odom"
final_msg.header.stamp = _time
final_msg.pose.pose.position.z = final_pose.point.z
final_msg.pose.covariance = msg.pose.covariance
self._range_pub.publish(final_msg)
def _obstacle_callback(self, msg):
try:
transform = self.tf_buffer.lookup_transform(
self.map_frame, msg.header.frame_id, msg.header.stamp,
rospy.Duration(1.0))
except (tf2.ConnectivityException, tf2.LookupException,
tf2.ExtrapolationException) as e:
rospy.logwarn(e)
return
for o in msg.obstacles:
point = PointStamped()
point.header = msg.header
point.point = o.pose.pose.pose.position
# Transfrom robot to map frame
point = tf2_geometry_msgs.do_transform_point(point, transform)
# Update robots that are close together
cleaned_robots = []
for old_robot in self.robots:
# Check if there is another robot in memory close to it
if np.linalg.norm(
ros_numpy.numpify(point.point) - ros_numpy.numpify(
old_robot.point)) > self.robot_merge_distance:
cleaned_robots.append(old_robot)
# Update our robot list with a new list that does not contain the duplicates
self.robots = cleaned_robots
# Append our new robots
self.robots.append(point)
def poses_callback(self, pose_msg, identity):
"""Callback for poses of other agents"""
rospy.loginfo_once('Got other poses callback')
if self.pose is None:
return
target_frame = f'drone_{self.my_id}/base_link'
source_frame = 'world'
try:
transform = self.buffer.lookup_transform(target_frame=target_frame,
source_frame=source_frame,
time=rospy.Time(0))
except Exception as e:
print(e)
return
point = PointStamped()
point.header = pose_msg.header
point.point.x = pose_msg.pose.position.x
point.point.y = pose_msg.pose.position.y
point.point.z = pose_msg.pose.position.z
point_tf = tf2_geometry_msgs.do_transform_point(point, transform)
p = point_tf.point
self.poses[identity] = np.array([p.x, p.y, p.z])
def free_space_srv_cb(req):
global map_data
global got_map
global tfBuffer
point = req.point
point_stamped = PointStamped()
# TODO: Fill in the infos of point_stamped
# Dont forget the header
# call the function tfBuffer.lookup_transform with the right params to
# get the transformation t
try:
point_in_map = tg.do_transform_point(point_stamped, t)
except Exception as e:
print e
return -1
while not got_map:
rospy.sleep(0.1)
ans = point_is_free(map_data, point_in_map)
if ans >= 80:
return 0
elif ans == -1:
return -1
else:
return 1
def get_mask_img(transform, target_bin, camera_model):
"""
:param point: point that is going to be transformed
:type point: PointStamped
:param transform: camera_frame -> bbox_frame
:type transform: Transform
"""
# check frame_id of a point and transform just in case
assert camera_model.tf_frame == transform.header.frame_id
assert target_bin.bbox.header.frame_id == transform.child_frame_id
transformed_list = [
do_transform_point(corner, transform)
for corner in target_bin.corners]
projected_points = project_points(transformed_list, camera_model)
# generate an polygon that covers the region
path = Path(projected_points)
x, y = np.meshgrid(
np.arange(camera_model.width),
np.arange(camera_model.height))
x, y = x.flatten(), y.flatten()
points = np.vstack((x, y)).T
mask_img = path.contains_points(
points).reshape(
camera_model.height, camera_model.width
).astype('bool')
return mask_img
def filter_frame(self, frame, trans, stamp):
"""
Applies a four-step algorithm to find all the roombas in a given BGR
image.
:param frame: raw BGR image in which to find roombas
:type frame: numpy.ndarray of shape 3 x WIDTH x HEIGHT with dtype uint8
:return: None
.. note::
Does not return data, only debugs the result
.. note::
The image origin is in the upper-left hand corner
"""
points = []
for img_coords in self.bound_roombas(frame):
cam_ray = pixel_to_ray(img_coords, frame.shape, self.daov)
# Convert that camera ray to world space (map frame)
# See note in script header about do_transform_vector3
map_ray = tf2_geometry_msgs.do_transform_vector3(
cam_ray, copy.deepcopy(trans))
# Scale the direction to hit the ground (plane z=0)
# Direction scale should always be positive
cam_pos = tf2_geometry_msgs.do_transform_point(
PointStamped(point=Point(0, 0, 0)), trans).point
direction_scale = -(cam_pos.z - ROOMBA_HEIGHT) / map_ray.vector.z
roomba_pos = Point()
roomba_pos.x = cam_pos.x + map_ray.vector.x * direction_scale
roomba_pos.y = cam_pos.y + map_ray.vector.y * direction_scale
roomba_pos.z = 0
# Debug the roomba line
points.append(trans.transform.translation)
points.append(roomba_pos)
# Add the roomba to array and publish
self.odom_lock.acquire()
sq_tolerance = 0.1 if len(self.odom_array.data) < 10 else 1000
for i in xrange(len(self.odom_array.data)):
pt = self.odom_array.data[i].pose.pose.position
if (roomba_pos.x - pt.x)**2 + \
(roomba_pos.y - pt.y)**2 < sq_tolerance:
self.odom_array.data[i].header.stamp = stamp
self.odom_array.data[i].pose.pose.position = roomba_pos
break
else:
item = Odometry()
item.child_frame_id = "roomba%d" % len(self.odom_array.data)
item.header.frame_id = "map"
item.header.stamp = stamp
item.pose.pose.position = roomba_pos
item.pose.pose.orientation.z = 1
self.odom_array.data.append(item)
self.publisher.publish(self.odom_array)
self.odom_lock.release()
self.debug.rviz_lines(points, 0)
def getTarget(): #in robot base frame
transf = tfBuffer.lookup_transform(BASE_FRAME,
MAP_FRAME,
rospy.Time(),
timeout=rospy.Duration(2))
print(transf)
tgt = tf2_geometry_msgs.do_transform_point(target, transf)
return np.array([tgt.point.x, tgt.point.y, 0])
def avoid_collision(setpoint):
global radius
transform = tf_buffer.lookup_transform(
robot_frame_id, setpoint.header.frame_id, rospy.Time(0), rospy.Duration(1))
transform_inverse = tf_buffer.lookup_transform(
setpoint.header.frame_id, robot_frame_id, rospy.Time(0), rospy.Duration(1))
point = tf2_geometry_msgs.do_transform_point(setpoint, transform)
goal = [point.point.x, point.point.y]
obstacles = get_obstacles()
control = orm.avoid_collision(goal, obstacles)
point.point.x = control[0]
point.point.y = control[1]
return tf2_geometry_msgs.do_transform_point(point, transform_inverse)
def transform_point(self, point, _from, _to):
'''Transforms point (geometry_msgs/PointStamped) from frame "_from" to
frame "_to".
Arguments:
- _from, _to = string, name of frame
'''
transform = self.kalman.get_transform(_from, _to)
point_transformed = tf2_geom.do_transform_point(point, transform)
return point_transformed
def move(path):
global control_client, robot_frame_id, pub
# Call service client (collision avoidance) with path
try:
service_result = control_client(path)
setpoint = service_result.setpoint
service_path = service_result.new_path
except:
print('false')
while service_path.poses: # This loop keeps going while the service_path is not empty
try:
service_result = control_client(service_path)
setpoint = service_result.setpoint
service_path = service_result.new_path
except:
print('error')
# Transform Setpoint from service client
trans = tf_buffer.lookup_transform(robot_frame_id,
setpoint.header.frame_id,
setpoint.header.stamp)
transformed_setpoint = tf2_geometry_msgs.do_transform_point(
setpoint, trans)
# Create Twist message from the transformed Setpoint
msg = Twist()
if math.atan2(transformed_setpoint.point.y,
transformed_setpoint.point.x) < max_angular_velocity:
msg.angular.z = math.atan2(transformed_setpoint.point.y,
transformed_setpoint.point.x)
else:
msg.angular.z = max_angular_velocity
if 0.5 * math.sqrt(
transformed_setpoint.point.x**2 +
transformed_setpoint.point.y**2) < max_linear_velocity:
msg.linear.x = msg.linear.x = 0.6 * math.sqrt(
transformed_setpoint.point.x**2 +
transformed_setpoint.point.y**2)
else:
msg.linear.x = max_linear_velocity
# Publish Twist
pub.publish(msg)
msg.linear.x = 0
msg.angular.z = 0
pub.publish(msg)
# Get new path from action server
get_path()
def led_image_location(self, image_blob):
loc = self.blob_location(image_blob)
if loc is None:
rospy.logwarn('Could not find average in point cloud')
return
(x, y, z, r, g, b) = loc
point_camera_frame = PointStamped(
point=Point(x=x, y=y, z=z),
header=image_blob.header)
# Publish points in camera optical frame for debugging
self.led_publisher_camera.publish(
PointStampedColorRGBA(
point_stamped=point_camera_frame,
color_rgba=ColorRGBA(r=r, g=g, b=b, a=1)))
# translate frame from left_camera_optical_frame to head
#
# Coordinates! This is my understanding...
#
# Camera optical frame coordinates are x right, y down, z ahead
# Head frame coordinates are x ahead, y left, z up
#
# The depth channel is:
# A large z value in the camera optical frame.
# A large x value in the head frame.
#
# Also expect sign change on the image plane coordinates as the
# camera is installed upside down.
#
# check for transform
if not self.tf_buffer.can_transform(self.target_frame,
point_camera_frame.header.frame_id,
point_camera_frame.header.stamp):
rospy.logwarn('No transform')
return
trans = self.tf_buffer.lookup_transform(
self.target_frame,
point_camera_frame.header.frame_id,
point_camera_frame.header.stamp)
point_head_frame = tf2_geometry_msgs.do_transform_point(
point_camera_frame,
trans)
color = ColorRGBA(r=r, g=g, b=b, a=1)
point_to_publish = PointStampedColorRGBA(
point_stamped=point_head_frame,
color_rgba=color)
rospy.loginfo('Head (x, y, z, r, g, b) (%f, %f, %f, %d, %d, %d)',
point_to_publish.point_stamped.point.x,
point_to_publish.point_stamped.point.y,
point_to_publish.point_stamped.point.z,
point_to_publish.color_rgba.r,
point_to_publish.color_rgba.g,
point_to_publish.color_rgba.b)
self.led_publisher.publish(point_to_publish)
def transformSetPoint(setPointResponse):
try:
rospy.loginfo("trying to transform from source %s to target %s", setPointResponse.setpoint.header.frame_id, "base_link")
trans = tfBuffer.lookup_transform("base_link", setPointResponse.setpoint.header.frame_id, setPointResponse.setpoint.header.stamp)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.logerr("transformation went wront")
return None
return tf2_geometry_msgs.do_transform_point(setPointResponse.setpoint, trans).point
def move(path):
global control_client, robot_frame_id, pub
while path.poses:
# Call service client with path
response_service = control_client(
path) # The service has one request: the path.
new_path = response_service.new_path # In exchange it gives you as response: setpoint and new path
setpoint = response_service.setpoint
# Transform Setpoint from service client
transform = tf_buffer.lookup_transform(
robot_frame_id, setpoint.header.frame_id,
rospy.Time()) # Create the transform matrix
transformed_setpoint = tf2_geometry_msgs.do_transform_point(
setpoint, transform) # Assign the transform matrix to the setpoint
# Create Twist message from the transformed Setpoint
Twist_msg = Twist()
# Assign the twist velocities as a direction vector toward the setpoint
Twist_msg.angular.z = -atan2(transformed_setpoint.point.y,
transformed_setpoint.point.x)
Twist_msg.linear.x = sqrt(transformed_setpoint.point.x**2 +
transformed_setpoint.point.y**2)
# Clip maximum velocities
# if(Twist_msg.linear.x >= 0):
Twist_msg.linear.x = min(max_linear_velocity, Twist_msg.linear.x)
#Twist_msg.angular.z =- min(max_angular_velocity,Twist_msg.angular.z)
'''
else:
Twist_msg.linear.x = max(-max_linear_velocity, Twist_msg.linear.x)
if(Twist_msg.angular.z >= 0):
Twist_msg.angular.z=min(max_angular_velocity, Twist_msg.angular.x)
else:
Twist_msg.angular.z=max(-max_angular_velocity, Twist_msg.angular.x)
'''
# Publish Twist
pub.publish(Twist_msg)
rate.sleep()
# Call service client again if the returned path is not empty and do stuff again
path = new_path
# Send 0 control Twist to stop robot
Twist_msg.angular.z = 0
Twist_msg.linear.x = 0
pub.publish(Twist_msg)
rate.sleep()
# Get new path from action server
get_path()
def lookAt(self, point):
# Create tf transformer for z point transformation to keep TIAGo's head on the same level
tfBuffer = tf2_ros.Buffer()
tf = tf2_ros.TransformListener(tfBuffer)
# Wait for the server to come up
rospy.loginfo('Waiting for the point head server to come up')
self.point_head_client.wait_for_server(rospy.Duration(10.0))
# Create the goal
print('Looking at: ', point)
ph_goal = PointHeadGoal()
ph_goal.target.header.frame_id = 'map'
ph_goal.max_velocity = 1
ph_goal.min_duration = rospy.Duration(0.5)
ph_goal.target.point.x = point[0]
ph_goal.target.point.y = point[1]
ph_goal.pointing_frame = 'head_2_link'
ph_goal.pointing_axis.x = 1
ph_goal.pointing_axis.y = 0
ph_goal.pointing_axis.z = 0
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = 'head_2_link'
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = tfBuffer.lookup_transform('base_link', 'head_2_link',
rospy.Time(0))
get_z_ps = do_transform_point(ps, transform)
transform_ok = True
# This usually happens only on startup
except tf2_ros.ExtrapolationException as e:
rospy.sleep(1.0 / 4)
ps.header.stamp = rospy.Time(0)
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ") at time " + str(ps.header.stamp))
except tf2_ros.LookupException:
pass
except tf2_ros.ConnectivityException:
pass
ph_goal.target.point.z = get_z_ps.point.z
print(get_z_ps.point.z)
# Send the goal
rospy.loginfo("Sending the goal...")
self.point_head_client.send_goal(ph_goal)
rospy.loginfo("Goal sent!!")
rospy.sleep(3)
def transform_points(self, conePos):
tf_cone_pos = []
for i in range(0,len(conePos)):
# transform points from camera_rgb_optical_frame to map frame
point_stamped = PointStamped()
point_stamped.point = conePos[i]
point_stamped.header = std_msgs.msg.Header()
point_stamped.header.stamp = rospy.Time.now()
point_stamped.header.frame_id = "camera_rgb_optical_frame"
transformed_point = tf2_geometry_msgs.do_transform_point(point_stamped, self.transform)
#print 'x,y,z', transformed_point.point.x,transformed_point.point.y, transformed_point.point.z
tf_cone_pos.append([transformed_point.point.x, transformed_point.point.y])
return tf_cone_pos
def mainloop(self):
# Set the rate of this loop
rate = rospy.Rate(2)
while not rospy.is_shutdown():
if self.goal:
try:
transform = self.tfBuffer.lookup_transform(
'world', 'tower', rospy.Time())
# msg = Vector3()
# msg.x = self.goal.x
# msg.y = self.goal.y
# msg.z = self.goal.z
# x = (msg.y * math.sin(0.523599)) + (msg.x * math.cos(0.523599))
# y = (msg.y * math.cos(0.523599)) - (msg.x * math.sin(0.523599))
# msg.x = round(x + (150*math.sin(0.506145)) - 0.370243036950555)
# msg.y = round(y - (150*math.cos(0.506145)) + 0.509702033064983)
# # msg.y = y*math.cos(0.523599) - x*math.sin(0.523599)
# # msg.x = y*math.sin(0.523599) + x*math.cos(0.523599)
# msg.z = 0
# Convert the goal to a PointStamped
point = PointStamped()
point.header.stamp = rospy.Time.now()
point.point.x = self.goal.x
point.point.y = self.goal.y
point.point.z = self.goal.z
# Use the do_transform_point function to convert the point using the transform
new_point = do_transform_point(point, transform)
# Convert the point back into a vector message containing intergers
msg = Vector3()
msg.x = new_point.point.x
msg.y = new_point.point.y
msg.z = new_point.point.z
# Publish the vector
rospy.loginfo(
str(rospy.get_name()) +
": Publishing Transformed Goal {}".format(
[msg.x, msg.y]))
self.goalpub.publish(msg)
# The tower will automatically send you a new goal once the drone reaches the requested position.
# Reset the goal
self.goal = None
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
rate.sleep()
def _balls_callback(self, msg):
try:
transform = self.tf_buffer.lookup_transform(
self.map_frame, msg.header.frame_id, msg.header.stamp,
rospy.Duration(1.0))
except (tf2.ConnectivityException, tf2.LookupException,
tf2.ExtrapolationException) as e:
rospy.logwarn(e)
return
point = PointStamped()
point.header = msg.header
point.point = msg.pose.pose.position
point = tf2_geometry_msgs.do_transform_point(point, transform)
self.ball = point
def transform_point(self, point):
"""
Convert a point to the local frame.
"""
point_stamped = PointStamped()
point_stamped.point.x = point[0]
point_stamped.point.y = point[1]
point_transformed_stamped = tf2_geometry_msgs.do_transform_point(
point_stamped, self.transform)
point_transformed = np.array([
point_transformed_stamped.point.x,
point_transformed_stamped.point.y
])
return point_transformed
def cb_scan(self, data):
"""
This callback runs each time a LIDAR scan is obtained from
the /scan topic in ROS. Returns a topic /detection. If no
object is found, /detection = [0,0]. If an object is found,
/detection = [angle,distance] to median of scan.
"""
self.target_pose = PoseWithCovarianceStamped()
# Set max/min angle and increment
scan_min = data.angle_min
scan_max = data.angle_max
scan_inc = data.angle_increment
#now = rospy.get_rostime()
scan_time = data.header.stamp.secs
# Build angle array
if self.physical_robot:
y = np.arange(scan_min,scan_max,scan_inc)-1.57
else:
y = np.arange(scan_min,scan_max+0.01*scan_inc,scan_inc)
# Pre-compute trig functions of angles
ysin = np.sin(y)
ycos = np.cos(y)
# Apply a median filter to the range scans
x = sg.medfilt(data.ranges,1)-1
# Calculate the difference between consecutive range values
x_diff1 = np.power(np.diff(x),2)
# Convert range and bearing measurement to cartesian coordinates
y_coord = x*ysin
x_coord = x*ycos
# Compute difference between consecutive values in cartesian coordinates
y_diff = np.power(np.diff(y_coord),2)
x_diff = np.power(np.diff(x_coord),2)
# Compute physical distance between measurements
dist = np.power(x_diff+y_diff,0.5)
# Segment the LIDAR scan based on physical distance between measurements
x2 = np.split(x,np.argwhere(dist>self.scan_dist_thresh))
y2 = np.split(y,np.argwhere(dist>self.scan_dist_thresh))
dist2 = np.split(dist,np.argwhere(dist>self.scan_dist_thresh))
x_coord2 = np.split(x_coord,np.argwhere(dist>self.scan_dist_thresh))
y_coord2 = np.split(y_coord,np.argwhere(dist>self.scan_dist_thresh))
ran2 = np.split(data.ranges,np.argwhere(dist>self.scan_dist_thresh))
bearing = np.array([0,0,0,0], dtype=np.float32)
for i in range(len(y2)):
# Check if there are at least 4 points in an object (reduces noise)
ylen = len(y2[i])-0
dist2_sum = np.sum(dist2[i][1:-2])
if ylen > self.ylen_lim and dist2_sum > self.dist_min and dist2_sum < self.dist_max and np.median(ran2[i]) <= 25:
x_pt = np.median(x_coord2[i])
y_pt = np.median(y_coord2[i])
if True:
ang = np.median(y2[i])
dis = np.median(x2[i])
mn = min(x2[i][1:ylen])
mx = max(x2[i][1:ylen])
dis = ((x_pt**2+y_pt**2))**0.5
if ang > self.ang_min and ang < self.ang_max:
sl = ylen
p = PointStamped()
p.point.x = x_pt
p.point.y = y_pt
p.point.z = 0.0
new_p = do_transform_point(p, self.R)
u = (dist-self.xmin)/(self.xmax-self.xmin)*(self.umax-self.umin)+self.umin
self.uvar = np.nanstd(u)**2
self.loc = (dis-self.xmin)/(self.xmax-self.xmin)*(self.locmax-self.locmin)+self.locmin
self.target_pose.pose.pose.position.x = new_p.point.x
self.target_pose.pose.pose.position.y = new_p.point.y
self.target_pose.pose.pose.position.z = self.loc
cov = self.target_pose.pose.covariance
self.target_pose.pose.covariance[0] = self.uvar
self.target_pose.pose.covariance[7] = self.uvar
cov[0] = 1.0*dis #self.uvar
cov[7] = 0.1*dis #self.uvar
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = 'base_link'
self.target_pose.header = h
else:
pass
#print('fail1')
else:
pass
#print('fail2')
else:
pass
#print('fail3',ylen,dist2_sum)
# Publish bearing to ROS on topic /detection
self.target_pub.publish(self.target_pose)
pass
