def __init__(self):
rospy.init_node('MissionManager')
self.Mission = MissionPlan.missionplan.Mission()
#print self.Mission;
# Variable for waypoint
self.waypoint = PointStamped()
# Variable for current position of boat--to be set in position_callback
self.currPosBoat_LatLon = GeoPointStamped()
self.currPosBoat_ECEF = PointStamped()
self.currPosBoat_Map = PointStamped()
# Set minimum distance to waypoint
self.threshold = 3.0
# Receive up-to-date UTM coordinates of boat
rospy.Subscriber('/position_utm', PoseStamped, self.position_utm_callback, queue_size = 1)
# Receive up-to-date lat/lon coordinates of boat
rospy.Subscriber('/position', GeoPointStamped, self.position_callback, queue_size = 1)
# To publish waypoint updates to MOOS
self.wpt_updates = rospy.Publisher('/moos/wpt_updates', String, queue_size = 10)
# TODO:
# Subscribe to waypoints/behaviors from AutonomousMissionPlanner. Not sure what topic.
pass
def forget_ball(self, own=True, team=True, reset_ball_filter=True):
"""
Forget that we and the best teammate saw a ball, optionally reset the ball filter
:param own: Forget the ball recognized by the own robot, defaults to True
:type own: bool, optional
:param team: Forget the ball received from the team, defaults to True
:type team: bool, optional
:param reset_ball_filter: Reset the ball filter, defaults to True
:type reset_ball_filter: bool, optional
"""
if own: # Forget own ball
self.ball_seen_time = rclpy.Time(seconds=0, nanoseconds=0)
self.ball = PointStamped()
if team: # Forget team ball
self.ball_seen_time_teammate = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_teammate = PointStamped()
if reset_ball_filter: # Reset the ball filter
result = self.reset_ball_filter()
if result.success:
self.get_logger().info(
f"Received message from ball filter: '{result.message}'",
logger_name='bitbots_blackboard')
else:
self.get_logger().warn(
f"Ball filter reset failed with: '{result.message}'",
logger_name='bitbots_blackboard')
def balls_cb(self, msg):
ball_msgs = msg.poses
for ball_msg in ball_msgs:
ball_point = PointStamped()
ball_point.header.frame_id = msg.header.frame_id
ball_point.header.stamp = msg.header.stamp
ball_point.point = ball_msg.pose.pose.position
try:
ball_point = self.tf_buffer.transform(ball_point,
"base_footprint")
except tf2_ros.LookupException:
rospy.logwarn("Could not transform from " +
msg.header.frame_id + " to 'base_footprint'")
return None
ball = QGraphicsEllipseItem(0, 0, self.ball_size, self.ball_size,
self.radar)
ball.setPen(self.ball_pen)
ball.setVisible(False)
ball.setOpacity(self.opacity)
self.set_scaled_position(ball, -1 * ball_point.point.x,
-1 * ball_point.point.y, self.ball_size,
self.ball_size)
color_tuple = self.confidence2color(ball_msg.confidence)
ball.setBrush(QBrush(QColor(*color_tuple)))
ball.setVisible(self.ball_active)
def transform(self, point, field, stamp):
K = self.camera_info.K
x = (point.x - K[2]) / K[0]
y = (point.y - K[5]) / K[4]
z = 1.0
point_on_image = np.array([x, y, z])
intersection = line_plane_intersection(field[0], field[1],
point_on_image)
if intersection is None:
return None
intersection_stamped = PointStamped()
intersection_stamped.point = intersection
intersection_stamped.header.stamp = stamp
intersection_stamped.header.frame_id = self.camera_info.header.frame_id
try:
intersection_transformed = self.tf_buffer.transform(
intersection_stamped, self.publish_frame)
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(
5.0, "Could not transform from " + self.publish_frame +
" to " + intersection_stamped.header.frame_id)
rospy.logwarn_throttle(5.0, ex)
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(
5.0, "Waiting for transforms to become available...")
rospy.logwarn_throttle(5.0, ex)
return None
return intersection_transformed.point
def get_detection_based_goal_position_uv(self):
"""
returns the position of the goal relative to the robot.
if only a single post is detected, the position of the post is returned.
else, it is the point between the posts
:return:
"""
left = PointStamped(self.goal_odom.header, self.goal_odom.left_post)
right = PointStamped(self.goal_odom.header, self.goal_odom.right_post)
left.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
right.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
try:
left_bfp = self.tf_buffer.transform(
left, self.base_footprint_frame,
timeout=Duration(seconds=0.2)).point
right_bfp = self.tf_buffer.transform(
right,
self.base_footprint_frame,
timeout=Duration(seconds=0.2)).point
except tf2.ExtrapolationException as e:
self.get_logger().warn(e)
self.get_logger().error(
'Severe transformation problem concerning the goal!')
return None
return (left_bfp.x + right_bfp.x / 2.0), \
(left_bfp.y + right_bfp.y / 2.0)
def _projectPointIntoFrame(self, point, source_frame_id, target_frame_id):
"""!
@brief Transform a point from one frame of reference to another.
@param point The geometry_msgs/Point to transform.
@param source_frame_id The fully resolved frame_id the point is in reference to.
@param target_frame_id The fully resolved frame_id to which the point should be
transformed.
@return The geometry_msgs/Point in the new frame of reference.
"""
# First, create a PointStamped, since that is what is needed to transform.
point_source_stamped = PointStamped()
point_source_stamped.header.stamp = rospy.Time.now()
point_source_stamped.header.frame_id = source_frame_id
point_source_stamped.point = point
# Do the transform. Give a timeout to let it wait for the latest TF if it would
# otherwise need to extrapolate.
timeout = rospy.Duration(self._tf_timeout)
try:
point_target_stamped = self._tf_buffer.transform(
object_stamped=point_source_stamped,
target_frame=target_frame_id,
timeout=timeout)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as error:
rospy.logerr(msg='TF transform error: {error}'.format(error=error))
raise
return point_target_stamped.point
def goal_callback(self, msg):
# type: (GoalRelative) -> None
self.goal = msg
# todo: transform to base_footprint too!
# adding a minor delay to timestamp to ease transformations.
self.goal.header.stamp = self.goal.header.stamp + rospy.Duration.from_sec(
0.01)
goal_left_buffer = PointStamped(self.goal.header, self.goal.left_post)
goal_right_buffer = PointStamped(self.goal.header,
self.goal.right_post)
self.goal_odom.header = self.goal.header
if goal_left_buffer.header.frame_id != 'odom':
try:
self.goal_odom.left_post = self.tf_buffer.transform(
goal_left_buffer, 'odom',
timeout=rospy.Duration(0.2)).point
self.goal_odom.right_post = self.tf_buffer.transform(
goal_right_buffer, 'odom',
timeout=rospy.Duration(0.2)).point
self.goal_odom.header.frame_id = 'odom'
self.goal_seen_time = rospy.Time.now()
except (tf2.ConnectivityException, tf2.LookupException,
tf2.ExtrapolationException) as e:
rospy.logwarn(e)
else:
self.goal_odom.left_post = goal_left_buffer.point
self.goal_odom.right_post = goal_right_buffer.point
self.goal_seen_time = rospy.Time.now()
def get_detection_based_goal_position_uv(self):
"""
returns the position of the goal relative to the robot.
if only a single post is detected, the position of the post is returned.
else, it is the point between the posts
:return:
"""
left = PointStamped(self.goal_odom.header, self.goal_odom.left_post)
right = PointStamped(self.goal_odom.header, self.goal_odom.right_post)
try:
left_bfp = self.tf_buffer.transform(
left, 'base_footprint', timeout=rospy.Duration(0.2)).point
right_bfp = self.tf_buffer.transform(
right, 'base_footprint', timeout=rospy.Duration(0.2)).point
except (tf2.ExtrapolationException) as e:
rospy.logwarn(e)
try:
# retrying with latest time stamp available because the time stamp of the goal_odom.header
# seems to be to young and an extrapolation would be required.
left.header.stamp = rospy.Time(0)
right.header.stamp = rospy.Time(0)
left_bfp = self.tf_buffer.transform(
left, 'base_footprint', timeout=rospy.Duration(0.2)).point
right_bfp = self.tf_buffer.transform(
right, 'base_footprint', timeout=rospy.Duration(0.2)).point
except (tf2.ExtrapolationException) as e:
rospy.logwarn(e)
rospy.logerr(
'Severe transformation problem concerning the goal!')
return None
return (left_bfp.x + right_bfp.x / 2.0), \
(left_bfp.y + right_bfp.y / 2.0)
def endPos(msg, rot):
# Copying for simplicity
position = msg.pose.position
quat = msg.pose.orientation
#Find difference between camera and suction end
diff = PointStamped()
diff.header.frame_id = str(
msg.header.frame_id) #camera_color_optical_frame
diff.header.stamp = rospy.Time(0)
diff.point.x = position.x #z
diff.point.y = position.y #x
diff.point.z = position.z #y
p = listener.transformPoint("panda_suction_end", diff)
pickPoint = PointStamped()
pickPoint.header.frame_id = "panda_suction_end"
pickPoint.header.stamp = rospy.Time(0)
pickPoint.point.x = p.point.x
pickPoint.point.y = p.point.y
pickPoint.point.z = p.point.z - suctionCup
p = listener.transformPoint("panda_link0", pickPoint)
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = rot[0]
pose_target.orientation.y = rot[1]
pose_target.orientation.z = rot[2]
pose_target.orientation.w = rot[3]
pose_target.position.x = p.point.x
pose_target.position.y = p.point.y
pose_target.position.z = p.point.z
if (pose_target.position.x > XMAX or pose_target.position.x < XMIN
or pose_target.position.y > YMAX or pose_target.position.y < YMIN):
rospy.loginfo("Error position out of bounderies")
return homePos()
return pose_target
def obstacle_cb(self, msg):
for item in self.obstacles:
self._scene.removeItem(item)
for obstacle_msg in msg.obstacles:
obstacle = QGraphicsEllipseItem(0, 0, self.obsacle_size,
self.obsacle_size, self.radar)
obstacle.setPen(self.obstacle_pen)
obstacle.setOpacity(self.opacity)
obstacle_point = PointStamped()
obstacle_point.header.frame_id = msg.header.frame_id
obstacle_point.header.stamp = msg.header.stamp
obstacle_point.point = obstacle_msg.pose.pose.pose.position
try:
obstacle_point = self.tf_buffer.transform(
obstacle_point, "base_footprint")
except tf2_ros.LookupException:
rospy.logwarn("Could not transform from " +
msg.header.frame_id + " to 'base_footprint'")
return None
self.set_scaled_position(obstacle, -1 * obstacle_point.point.x,
-1 * obstacle_point.point.y,
self.obsacle_size, self.obsacle_size)
color_tuple = self.confidence2color(obstacle_msg.pose.confidence)
obstacle.setBrush(QBrush(QColor(*color_tuple)))
obstacle.setVisible(True)
self.obstacles.append(obstacle)
def endPos(msg,rot):
# Copying for simplicity
position = msg.pose.position
quat = msg.pose.orientation
#Find difference between camera and suction end
diff=PointStamped()
diff.header.frame_id = str(msg.header.frame_id) #camera_color_optical_frame
diff.header.stamp = rospy.Time(0)
diff.point.x=position.x #z
diff.point.y=position.y #x
diff.point.z=position.z #y
p=listener.transformPoint("panda_suction_end",diff)
pickPoint=PointStamped()
pickPoint.header.frame_id = "panda_suction_end"
pickPoint.header.stamp = rospy.Time(0)
pickPoint.point.x=p.point.x
pickPoint.point.y=p.point.y
pickPoint.point.z=p.point.z-suctionCup
p = listener.transformPoint("panda_link0",pickPoint)
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = rot[0]
pose_target.orientation.y = rot[1]
pose_target.orientation.z = rot[2]
pose_target.orientation.w = rot[3]
pose_target.position.x = p.point.x
pose_target.position.y = p.point.y
pose_target.position.z = p.point.z
currentbox = [quat.x, quat.y, quat.z, quat.w]
print(currentbox)
return pose_target, quat
def __init__(self):
rospy.init_node('MissionManager')
# This will be used to read mission.txt file later...
self.Mission = MissionPlan.missionplan.Mission()
self.missionReader = MissionReader.MissionReader()
# Variable for waypoint - in map / robot coordinates
self.waypoint = PointStamped()
# Variable for current position of boat--to be set in position_callback
self.currPosBoat_LatLon = GeoPointStamped()
self.currPosBoat_ECEF = PointStamped()
self.currPosBoat_Map = PointStamped()
# TODO: Change depth type when we find out what it should be.
self.depth = Float32
self.wptIndex = Int32
# TODO: Threshold possibly set thresholds in parameters?
# Set minimum distance to waypoint
self.shallowWaterDanger = False
self.waypointThreshold = 3.0
self.depthThreshold = 4.0
self.shallowWaterNegativeCount = 0
self.shallowWaterPositiveCount = 0
# If ASV receives this many consecutive negative shallow warnings,
# reset positive shallowWater counts
self.shallowWaterNegativeCountThreshold = 5
# If ASV receives this many consecutive positive shallow warnings,
# deviate path and reset negative shallowWater counts
self.shallowWaterPositiveCountThreshold = 5
#self.xIndex = 1
self.moos_wptIndex_Update = False
self.shallowWaterUpdate = False
self.incrementedLine = False
# Timer used to print status
self.everyTwoSeconds = rospy.Duration(0.5)
# Receive up-to-date UTM coordinates of ASV
rospy.Subscriber('/position_utm', PoseStamped, self.position_utm_callback, queue_size = 1)
# Receive up-to-date lat/lon coordinates of ASV
rospy.Subscriber('/position', GeoPointStamped, self.position_callback, queue_size = 1)
# Receive up-to-date depth
rospy.Subscriber('/depth', Float32, self.depth_callback, queue_size = 1)
#Receive up-to-date warnings from Don't Run Aground, Ben! behavior.
rospy.Subscriber('/shallow_water_warning', String, self.shallow_water_callback, queue_size = 1)
# Receive up-to-date waypoint indices
rospy.Subscriber('/moos/wpt_index', Int32, self.waypoint_index_callback, queue_size = 1)
# Receive up-to-date waypoint updates
rospy.Subscriber('/moos/wpt_updates', String, self.waypoint_updates_callback, queue_size = 1)
# To publish waypoint updates to MOOS
self.wpt_index_publisher = rospy.Publisher('/moos/wpt_index', Int32, queue_size = 10)
self.wpt_updates_publisher = rospy.Publisher('/moos/wpt_updates', String, queue_size = 10)
self.behavior_ctrl = rospy.Publisher('/behavior_ctrl', BehaviorControl, queue_size = 10)
def publish_marker(self, e):
# construct PoseWithCertaintyArray() message for map frame
ball_absolute = PoseWithCertainty()
ball_absolute.pose.pose = self.pose
ball_absolute.confidence = 1.0
balls_absolute = PoseWithCertaintyArray()
balls_absolute.header.stamp = rospy.get_rostime()
balls_absolute.header.frame_id = "map"
balls_absolute.poses = [ball_absolute]
# publish the new ball position
if self.publish:
self.absolute_publisher.publish(balls_absolute)
# check if ball is also visible for the robot and publish on relative topic if this is the case
# only works if camera info is provided
if self.cam_info:
try:
ball_point_stamped = PointStamped()
ball_point_stamped.header.stamp = rospy.Time.now()
ball_point_stamped.header.frame_id = "map"
ball_point_stamped.point = ball_absolute.pose.pose.position
ball_in_camera_optical_frame = tf_buffer.transform(
ball_point_stamped,
self.cam_info["frame_id"],
timeout=rospy.Duration(0.5))
if ball_in_camera_optical_frame.point.z >= 0:
p = [
ball_in_camera_optical_frame.point.x,
ball_in_camera_optical_frame.point.y,
ball_in_camera_optical_frame.point.z
]
k = np.reshape(self.cam_info["K"], (3, 3))
p_pixel = np.matmul(k, p)
p_pixel = p_pixel * (1 / p_pixel[2])
# make sure that the transformed pixel is inside the resolution and positive.
if 0 < p_pixel[0] <= self.cam_info["width"] and 0 < p_pixel[
1] <= self.cam_info["height"]:
ball_in_footprint_frame = tf_buffer.transform(
ball_in_camera_optical_frame,
"base_footprint",
timeout=rospy.Duration(0.5))
ball_relative = PoseWithCertainty()
ball_relative.pose.pose.position = ball_in_footprint_frame.point
ball_relative.confidence = 1.0
balls_relative = PoseWithCertaintyArray()
balls_relative.header = ball_in_footprint_frame.header
balls_relative.poses = [ball_relative]
self.relative_publisher.publish(balls_relative)
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
def transform(self, value):
point_new = geometry_msgs.msg.PointStamped()
point_new.header = value.req.header
point_new.point = value.req.point
transformed = self.tfBuffer.transform(point_new, self.camera_model.tfFrame())
transformed_new = PointStamped()
transformed_new.header = transformed.header
transformed_new.point = transformed.point
return transformed_new
def detect_fun(self):
found = False
sherd_poses = [] # initialize empty
# confirm that color mask exists
if self.color_mask is None:
rospy.logwarn('AutoCore: No color mask received.')
return found, sherd_poses
# run segment_sherds.py on what robot sees in this position
req = SherdDetectionsRequest()
req.color_mask = self.color_mask
try:
res = self.detection_srv(req)
except rospy.ServiceException as e:
rospy.logerr(
'AutoCore: Bounding box service call failed: {}'.format(e))
raise
else:
detections = res.detections.detections
rospy.logdebug('Bounding boxes message: {}'.format(detections))
if detections:
found = True
# TODO return (first or random or smallest etc.) sherd from list
for item in detections:
point_cam = PointStamped() # build ROS message for conversion
point_cam.header = item.header
point_cam.point.x, point_cam.point.y, point_cam.point.z = item.bbox.center.position.x, item.bbox.center.position.y, item.bbox.center.position.z
try:
point_base = self.tfBuffer.transform(
point_cam, 'base_link')
except tf2_ros.buffer_interface.TypeException as e:
rospy.logerr('AutoCore: tf failure: {}'.format(e))
raise
rospy.logdebug(
'Obtained transform between camera_link and base_link.')
rospy.logdebug(
'Sherd center point (x,y,z) [m] in base_link frame: {}'.
format(point_base))
sherd_poses.append({
'position':
np.array([
point_base.point.x, point_base.point.y,
point_base.point.z
]),
'roll':
item.bbox.center.roll
})
rospy.logdebug('sherds list = {}'.format(sherd_poses))
return found, sherd_poses
def write_circles(self, data):
object_frame = rospy.get_param("~object_frame")
global_frame = rospy.get_param(
"~global_frame") #NE MOZE MAPA BITI OBJECT...
tf = self.tfBuffer.lookup_transform(global_frame, object_frame,
rospy.Time(0))
orientation = tf.transform.rotation
quat_list = [
orientation.x, orientation.y, orientation.z, orientation.w
]
(roll, pitch, yaw) = euler_from_quaternion(quat_list)
# print yaw
newData = Obstacles() #for publishing transformed circles
newData.circles = []
newData.header = data.header
newData.header.frame_id = global_frame
self.circles = []
temp_point_stamped = PointStamped()
for circle in data.circles:
# x = self.pioneerPose.position.x + math.cos(yaw) * circle.center.x + math.sin(yaw) * circle.center.y
# y = self.pioneerPose.position.y - math.sin(yaw) * circle.center.x + math.cos(yaw) * circle.center.y
# circle.center.x = x
# circle.center.y = y
temp_point_stamped.point = circle.center
new_center = tf2_geometry_msgs.do_transform_point(
temp_point_stamped, tf)
circle.center = new_center.point
self.circles.append(circle)
newData.circles.append(circle)
self.transformed_obstacles.publish(newData)
self.circles = data.circles
max_num_of_no_actor = rospy.get_param("~max_num_of_no_actor")
self.header = data.header
self.header.frame_id = global_frame
if self.actor != None:
self.update_actor()
if self.count >= max_num_of_no_actor:
self.stop_robot()
rospy.signal_shutdown("Actor has been lost!!!")
def get_plane(self, stamp, object_height, base_frame):
""" returns a plane which an object is believed to be on as a tuple of a point on this plane and a normal"""
field_normal = PointStamped()
field_normal.header.frame_id = base_frame
field_normal.header.stamp = stamp
field_normal.point.x = 0.0
field_normal.point.y = 0.0
field_normal.point.z = 1.0
try:
field_normal = self.tf_buffer.transform(
field_normal,
self.camera_info.header.frame_id,
timeout=rospy.Duration(0.2))
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(
5.0, "Could not transform from " + base_frame + " to " +
self.camera_info.header.frame_id)
rospy.logwarn_throttle(5.0, ex)
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(
5.0, "Waiting for transforms to become available...")
rospy.logwarn_throttle(5.0, ex)
return None
field_point = PointStamped()
field_point.header.frame_id = base_frame
field_point.header.stamp = stamp
field_point.point.x = 0.0
field_point.point.y = 0.0
field_point.point.z = object_height
try:
field_point = self.tf_buffer.transform(
field_point, self.camera_info.header.frame_id)
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(
5.0, "Could not transform from " + base_frame + " to " +
self.camera_info.header.frame_id)
rospy.logwarn_throttle(5.0, ex)
return None
field_normal = np.array(
[field_normal.point.x, field_normal.point.y, field_normal.point.z])
field_point = np.array(
[field_point.point.x, field_point.point.y, field_point.point.z])
# field normal is a vector! so it stats at field point and goes up in z direction
field_normal = field_point - field_normal
return field_normal, field_point
def ball_filtered_callback(self, msg: PoseWithCovarianceStamped):
self.ball_filtered = msg
# When the precision is not sufficient, the ball ages.
x_sdev = msg.pose.covariance[0] # position 0,0 in a 6x6-matrix
y_sdev = msg.pose.covariance[7] # position 1,1 in a 6x6-matrix
if x_sdev > self.body_config['ball_position_precision_threshold']['x_sdev'] or \
y_sdev > self.body_config['ball_position_precision_threshold']['y_sdev']:
self.forget_ball(own=True, team=False, reset_ball_filter=False)
return
ball_buffer = PointStamped(msg.header, msg.pose.pose.position)
try:
self.ball = self.tf_buffer.transform(ball_buffer,
self.base_footprint_frame,
timeout=Duration(seconds=0.3))
self.ball_odom = self.tf_buffer.transform(
ball_buffer, self.odom_frame, timeout=Duration(seconds=0.3))
self.ball_map = self.tf_buffer.transform(
ball_buffer, self.map_frame, timeout=Duration(seconds=0.3))
# Set timestamps to zero to get the newest transform when this is transformed later
self.ball_odom.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_map.header.stamp = rclpy.Time(seconds=0, nanoseconds=0)
self.ball_seen_time = self.get_clock().now()
self.ball_publisher.publish(self.ball)
self.ball_seen = True
except (tf2.ConnectivityException, tf2.LookupException,
tf2.ExtrapolationException) as e:
self.get_logger().warn(e)
def goToGoal(self, goal_position):
stamped_goal = PointStamped()
stamped_goal.header.frame_id = "odom"
stamped_goal.point.x = goal_position.x
stamped_goal.point.y = goal_position.y
# Attend exercise requirements
tf_buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tf_buffer)
rate = rospy.Rate(10.0)
while not self.reachTheGoal():
try:
trans = tf_buffer.transform(stamped_goal, "base_link")
except:
rate.sleep()
print "Error getting transform"
continue
vel_msg = Twist()
print "vel_msg.linear.x = " + str(
self.linearVelocity(trans)) + " vel_msg.angular.z = " + str(
self.angularVelocity(trans))
vel_msg.linear.x = self.linearVelocity(trans)
vel_msg.angular.z = self.angularVelocity(trans)
self.velocity_publisher.publish(vel_msg)
rate.sleep()
def __init__(self, field_length, field_width, goal_width):
self.position = Position2D()
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(30))
self.tf_listener = tf2.TransformListener(self.tf_buffer)
self.ball = PointStamped() # The ball in the base footprint frame
self.goal = GoalRelative()
self.goal_odom = GoalRelative()
self.goal_odom.header.stamp = rospy.Time.now()
self.goal_odom.header.frame_id = 'odom'
self.obstacles = ObstaclesRelative()
self.my_data = dict()
self.counter = 0
self.ball_seen_time = rospy.Time(0)
self.goal_seen_time = rospy.Time(0)
self.ball_seen = False
self.field_length = field_length
self.field_width = field_width
self.goal_width = goal_width
# Publisher for Visualisation in RViZ
self.ball_publisher = rospy.Publisher('/debug/viz_ball',
PointStamped,
queue_size=1)
self.goal_publisher = rospy.Publisher('/debug/viz_goal',
GoalRelative,
queue_size=1)
def handle_gaze_controller(self, event):
with self.lock:
try:
aaa = PointStamped()
aaa.header.stamp = rospy.Time()
aaa.header.frame_id = self.target.target_point.header.frame_id
aaa.point.x = self.target.target_point.point.x
aaa.point.y = self.target.target_point.point.y
aaa.point.z = self.target.target_point.point.z
point_transformed = self.tf_buf.transform(aaa, 'head_base')
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
e = sys.exc_info()[0]
rospy.logdebug(e)
rospy.logwarn(
"[%s] Can't tranform from gaze to target.[ %s - %s ]" %
(rospy.get_name(), 'gaze',
self.target.target_point.header.frame_id))
return
pan_angle = math.atan2(point_transformed.point.y,
point_transformed.point.x)
tilt_angle = math.atan2(point_transformed.point.z,
point_transformed.point.x)
# print pan_angle * 180 / math.pi, tilt_angle * 180 / math.pi
if self.enable_gaze:
self.pub_pan.publish(pan_angle)
self.pub_tilt.publish(tilt_angle)
def __init__(self):
self.lock = threading.RLock()
self.tf_buf = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buf)
self.prev_head_pan = 0.0
self.enable_gaze_sync = True
self.result_pan_angle = 0.0
self.result_tilt_angle = 0.0
self.sub_gaze_sync = rospy.Subscriber('set_gaze_sync', Bool,
self.handle_set_gaze_sync)
self.sub_gaze_target = rospy.Subscriber('set_gaze_target',
PointStamped,
self.handle_set_gaze_target)
self.pub_head_pan = rospy.Publisher('/head/pan_controller/command',
Float64,
queue_size=10)
self.pub_head_tilt = rospy.Publisher('/head/tilt_controller/command',
Float64,
queue_size=10)
with self.lock:
self.head_target_point = PointStamped()
self.head_target_point.header.frame_id = "base_footprint"
self.head_target_point.point.x = 2.0
self.head_target_point.point.y = 0.0
self.head_target_point.point.z = 1.291
rospy.Timer(rospy.Duration(0.02), self.handle_head_controller)
rospy.loginfo('[%s] initialzed...' % rospy.get_name())
def _transform_point(self, point: Point, field, stamp) -> Point:
np_point = self._get_field_intersection_for_pixels(
np.array([[point.x, point.y, point.z]]), field)[0]
if np.isnan(np_point).any():
return None
intersection_stamped = PointStamped()
intersection_stamped.point.x = np_point[0]
intersection_stamped.point.y = np_point[1]
intersection_stamped.point.z = np_point[2]
intersection_stamped.header.stamp = stamp
intersection_stamped.header.frame_id = self._camera_info.header.frame_id
try:
intersection_transformed = self._tf_buffer.transform(
intersection_stamped, self._publish_frame)
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex))
return None
return intersection_transformed.point
def gripper_in_odom(self):
success = False
# Point of gripper origin.
gripper_position = PointStamped()
# TODO Should this be 'time'?
gripper_position.header.stamp = rospy.Time.now()
gripper_position.header.frame_id = "wrist_3_link"
gripper_position.point.x = 0
gripper_position.point.y = 0
gripper_position.point.z = 0
print("Created gripper origin for transformation.")
try:
# TODO Need to deal with if time is 0 because the clock hasn't been published yet(?)? - http://wiki.ros.org/roscpp/Overview/Time
# Times out after 1 second. What happens if the buffer doesn't contain the
# transformation after this duration? [I think it throws an error]
# --> Even works with a duration of 0.001 - how does this work? TODO
gripper_odom = self.tf_buffer.transform(gripper_position, 'odom',
rospy.Duration(1))
print("Performed transform")
success = True
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException) as e:
# TODO Need to add more details to this. Might just be doing it because the buffer isn't large enough yet.
# TODO Might need to revise this error message now that I've chopped things around.
print("Strategy TF transform error. Message:: {}".format(e))
# TODO Return success variable?
return [gripper_odom.point.x, gripper_odom.point.y]
def get_plane(self, stamp, object_height):
""" returns a plane which an object is believed to be on as a tuple of a point on this plane and a normal"""
base_frame = self._base_footprint_frame
field_normal = PointStamped()
field_normal.header.frame_id = base_frame
field_normal.header.stamp = stamp
field_normal.point.x = 0.0
field_normal.point.y = 0.0
field_normal.point.z = 1.0
try:
field_normal = self._tf_buffer.transform(
field_normal,
self._camera_info.header.frame_id,
timeout=rospy.Duration(0.2))
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex))
return None
field_point = PointStamped()
field_point.header.frame_id = base_frame
field_point.header.stamp = stamp
field_point.point.x = 0.0
field_point.point.y = 0.0
field_point.point.z = object_height
try:
field_point = self._tf_buffer.transform(
field_point, self._camera_info.header.frame_id)
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex))
return None
field_normal = np.array(
[field_normal.point.x, field_normal.point.y, field_normal.point.z])
field_point = np.array(
[field_point.point.x, field_point.point.y, field_point.point.z])
# field normal is a vector! so it stats at field point and goes up in z direction
field_normal = field_point - field_normal
return field_normal, field_point
def draw_goal_part(self, obj, msg, point, size):
goal_point = PointStamped()
goal_point.header.frame_id = msg.header.frame_id
goal_point.header.stamp = msg.header.stamp
goal_point.point = point
try:
goal_point = self.tf_buffer.transform(goal_point, "base_footprint")
except tf2_ros.LookupException:
rospy.logwarn("Could not transform from " + msg.header.frame_id +
" to 'base_footprint'")
return None
self.set_scaled_position(obj, -1 * goal_point.point.x,
-1 * goal_point.point.y, size, size)
color_tuple = self.confidence2color(msg.confidence)
obj.setBrush(QBrush(QColor(*color_tuple)))
obj.setVisible(self.goal_active)
def perform(self, connector, reevaluate=False):
point = PointStamped()
point.point.x = self.u
point.point.y = self.v
point.point.z = self.w
point.header.frame_id = 'base_footprint'
self.look_at(point, connector)
def msg_to_pos(tf_buffer, msg, to_frame_id, scale=1.0):
if msg is None:
return None
ps = PointStamped()
ps.header = msg.header
ps.point.x = msg.pose.pose.position.x * scale
ps.point.y = msg.pose.pose.position.y * scale
ps.point.z = msg.pose.pose.position.z * scale
try:
ps2 = tf_buffer.transform(ps, to_frame_id)
except Exception as e:
rospy.logwarn('Exception during TF from {:s} to {:s}: {:s}'.format(
msg.header.frame_id, to_frame_id, e))
return None
return np.array([ps2.point.x, ps2.point.y, ps2.point.z])
def ball_cb(self, msg):
ball_point = PointStamped()
ball_point.header.frame_id = msg.header.frame_id
ball_point.header.stamp = msg.header.stamp
ball_point.point = msg.ball_relative
try:
ball_point = self.tf_buffer.transform(ball_point, "base_footprint")
except tf2_ros.LookupException:
rospy.logwarn("Could not transform from " + msg.header.frame_id +
" to 'base_footprint'")
return None
if msg.ball_relative is not None:
self.set_scaled_position(self.ball, -1 * ball_point.point.x,
-1 * ball_point.point.y, self.ball_size,
self.ball_size)
color_tuple = self.confidence2color(msg.confidence)
self.ball.setBrush(QBrush(QColor(*color_tuple)))
self.ball.setVisible(self.ball_active)
def balls_relative_cb(self, msg):
if msg.poses:
balls = sorted(msg.poses, reverse=True, key=lambda ball: ball.confidence) # Sort all balls by confidence
ball = balls[0] # Ball with highest confidence
# Get ball relative to base footprint
ball_obj = PointStamped()
ball_obj.header = msg.header
ball_obj.point = ball
x, y = self.get_ball_position_uv(ball_obj)
self.ball_position = (float(x), float(y))
# Set ball distance
self.ball_distance = math.sqrt(float(self.ball_position[0])**2 + float(self.ball_position[1])**2)
# Calculate the angle in which we are facing the ball
self.ball_angle = math.atan2(float(self.ball_position[1]),float(self.ball_position[0]))
# Set the refresh time
self.ball_age = time.time()
else:
rospy.logwarn("Received empty balls message")
