def callback(data):
global prev_state
global predicted_goal
global time_since_prediction
global confidence
global tf_listener
global new_test
publish_current_pose() # we always want to publish this and update it
# first check if it has been a second since the last update
if predicted_goal is not None and time.time() - time_since_prediction > 1.:
predicted_goal = None
confidence = 0.
execution_time = 0.5
new_pose = PoseStamped()
new_pose.header.stamp = rospy.Time.now()
new_pose.header.frame_id = 'ee_link'
# if enter is pressed, reset to the start position
if new_test:
seconds = rospy.get_time()
seconds -= 0.1
now = rospy.Time.from_sec(seconds)
(trans, rot) = tf_listener.lookupTransform("/world", "/ee_link", now)
point = PointStamped()
point.header.frame_id = "/world"
point.header.stamp = now
point.point = start_position_to_return
target_pos = tf_listener.transformPoint('/ee_link', point).point
optimal_goal_direction = get_normalized_vector(target_pos)
new_pose.pose.position.x = optimal_goal_direction.x / 100.
new_pose.pose.position.y = optimal_goal_direction.y / 100.
new_pose.pose.position.z = optimal_goal_direction.z / 100.
new_pose.pose.orientation.w = 1.0
publish_pose(new_pose, execution_time)
if get_magnitude(target_pos) < 0.02:
new_test = False
return
if abs(data.x) < 0.05 and abs(data.y) < 0.05 and abs(data.z) < 0.05:
return
if predicted_goal is not None:
"""
# rospy.loginfo(predicted_goal)
# rospy.loginfo(group.get_current_pose().pose.position)
optimal_goal_direction = get_vector(convert_to_ur_coord(predicted_goal),
group.get_current_pose().pose.position)
optimal_goal_direction = get_normalized_vector(optimal_goal_direction)
keyboard_magnitude = get_magnitude(data)
optimal_goal_direction.x *= keyboard_magnitude
optimal_goal_direction.y *= keyboard_magnitude
optimal_goal_direction.z *= keyboard_magnitude
rospy.loginfo("predicted")
rospy.loginfo(convert_to_ur_coord(predicted_goal))
rospy.loginfo("current pos")
rospy.loginfo(group.get_current_pose())
# rospy.loginfo("The goal is in x: " + str(predicted_goal.x))
# rospy.loginfo("Current pos is x: " + str(group.get_current_pose().pose.position.y))
# rospy.loginfo("Current fixed x: " + str(convert_to_ur_coord(predicted_goal).x))
new_pose.pose.position.x = data.x / 100. * (1 - confidence) + optimal_goal_direction.x / 100. * confidence
new_pose.pose.position.y = data.y / 100. * (1 - confidence) + optimal_goal_direction.y / 100. * confidence
new_pose.pose.position.z = data.z / 100. # * (1 - confidence) + optimal_goal_direction.z / 100. * confidence
new_pose.pose.orientation.w = 1.0
"""
# predicted goal pos in ee_link space
try:
# now = rospy.Time.now()
seconds = rospy.get_time()
seconds -= 0.1
now = rospy.Time.from_sec(seconds)
(trans,
rot) = tf_listener.lookupTransform("/camera_depth_optical_frame",
"/ee_link", now)
#position is
point = PointStamped()
point.header.frame_id = '/camera_depth_optical_frame'
point.header.stamp = now
point.point = predicted_goal
target_pos = tf_listener.transformPoint('/ee_link', point).point
optimal_goal_direction = get_normalized_vector(target_pos)
keyboard_magnitude = get_magnitude(data)
optimal_goal_direction.x *= keyboard_magnitude
optimal_goal_direction.y *= keyboard_magnitude
optimal_goal_direction.z *= keyboard_magnitude
new_pose.pose.position.x = data.x / 100. * (
1 - confidence) + optimal_goal_direction.x / 100. * confidence
new_pose.pose.position.y = data.y / 100. * (
1 - confidence) + optimal_goal_direction.y / 100. * confidence
new_pose.pose.position.z = data.z / 100. # * (1 - confidence) + optimal_goal_direction.z / 100. * confidence
new_pose.pose.orientation.w = 1
# rospy.logerr("Before publish")
publish_pose(new_pose, execution_time)
except (tf.LookupException, tf.ConnectivityException):
rospy.logwarn("Exception on lookupTransform")
else:
new_pose.pose.position.x = data.x / 100.
new_pose.pose.position.y = data.y / 100.
new_pose.pose.position.z = data.z / 100.
new_pose.pose.orientation.w = 1.0
publish_pose(new_pose, execution_time)
def publish_tracked_objects(self, now):
"""
Publish markers of tracked objects to Rviz
"""
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame,
self.fixed_frame, tf_time,
rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(
self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo(
"Person tracker: tf not avaiable. Not publishing people")
else:
for track in self.objects_tracked:
if track.is_person:
continue
if self.publish_occluded or track.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
# Get the track position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = track.pos_x
ps.point.y = track.pos_y
try:
ps = self.listener.transformPoint(
self.publish_people_frame, ps)
except:
continue
# publish rviz markers
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "objects_tracked"
if track.in_free_space < self.in_free_space_threshold:
marker.color.r = track.colour[0]
marker.color.g = track.colour[1]
marker.color.b = track.colour[2]
else:
marker.color.r = 0
marker.color.g = 0
marker.color.b = 0
marker.color.a = 1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.id = marker_id
marker_id += 1
marker.type = Marker.CYLINDER
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.2
marker.pose.position.z = 0.15
self.marker_pub.publish(marker)
# # Publish a marker showing distance travelled:
# if track.dist_travelled > 1:
# marker.color.r = 1.0
# marker.color.g = 1.0
# marker.color.b = 1.0
# marker.color.a = 1.0
# marker.id = marker_id
# marker_id += 1
# marker.type = Marker.TEXT_VIEW_FACING
# marker.text = str(round(track.dist_travelled,1))
# marker.scale.z = 0.1
# marker.pose.position.z = 0.6
# self.marker_pub.publish(marker)
# Publish <self.confidence_percentile>% confidence bounds of track as an ellipse:
# cov = track.filtered_state_covariances[0][0] + track.var_obs # cov_xx == cov_yy == cov
# std = cov**(1./2.)
# gate_dist_euclid = scipy.stats.norm.ppf(1.0 - (1.0-self.confidence_percentile)/2., 0, std)
# marker.type = Marker.SPHERE
# marker.scale.x = 2*gate_dist_euclid
# marker.scale.y = 2*gate_dist_euclid
# marker.scale.z = 0.01
# marker.color.r = 1.0
# marker.color.g = 1.0
# marker.color.b = 1.0
# marker.color.a = 0.1
# marker.pose.position.z = 0.0
# marker.id = marker_id
# marker_id += 1
# self.marker_pub.publish(marker)
# Clear previously published track markers
for m_id in xrange(marker_id, self.prev_track_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "objects_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_track_marker_id = marker_id
def publish(dx, dy, stamp, pub):
p = PointStamped()
p.header.stamp = stamp
p.point.x = dx
p.point.y = dy
pub.publish(p)
def __init__(self):
self.n_obs = 2
print('Start node')
# Initialize node
rospy.init_node('ellipse_publisher', anonymous=True)
rate = rospy.Rate(200) # Frequency
rospy.on_shutdown(self.shutdown)
# Create publishers
self.pub_vel = rospy.Publisher('lwr/joint_controllers/passive_ds_command_vel', Twist, queue_size=5)
pub_orient = rospy.Publisher('lwr/joint_controllers/passive_ds_command_orient', Quaternion, queue_size=5)
pub_cent = [0]*self.n_obs
pub_cent[0] = rospy.Publisher("obs0_cent", PointStamped, queue_size=5)
pub_cent[1] = rospy.Publisher("obs1_cent", PointStamped, queue_size=5)
# Create listener
pose_sub = [0]*self.n_obs
pose_sub[0] = rospy.Subscriber("obstacle0", Obstacle, self.callback_obs0)
pose_sub[1] = rospy.Subscriber("obstacle1", Obstacle, self.callback_obs1)
basket_sub = rospy.Subscriber("basket", Obstacle, self.callback_basket)
convey_sub = rospy.Subscriber("convey", Obstacle, self.callback_convey)
attr_sub = rospy.Subscriber("attractor", PoseStamped, self.callback_attr)
self.listener = tf.TransformListener() # TF listener
self.obs = [0]*self.n_obs
self.pos_obs=[0]*self.n_obs
self.quat_obs=[0]*self.n_obs
# Set watiing variables true
print('wait obstacle')
self.awaitObstacle = [True for i in range(self.n_obs)]
self.awaitAttr = True
self.obs_basket=[]
self.awaitBasket = True
self.obs_convey=[]
self.awaitConvey = True
while any(self.awaitObstacle):
rospy.sleep(0.1) # Wait zzzz* = 1 # wait
while self.awaitBasket:
print('Waiting for basket obstacle')
rospy.sleep(0.1) # Wait zzzz* = 1 # wait
while self.awaitConvey:
print('Waiting for conveyer obstacle')
rospy.sleep(0.1) # Wait zzzz* = 1 # wait
while self.awaitAttr:
rospy.sleep(0.1)
print('got it all')
# Get initial transformation
rospy.loginfo('Getting Transformationss.')
awaitingTrafo_ee=True
awaitingTrafo_obs= [True] * self.n_obs
awaitingTrafo_attr=True
for i in range(self.n_obs):
while awaitingTrafo_obs[i]: # Trafo obs1 position
try:
self.pos_obs[i], self.quat_obs[i] = self.listener.lookupTransform("/world", "/object_{}/base_link".format(i), rospy.Time(0))
awaitingTrafo_obs[i]=False
except:
rospy.loginfo('Waiting for obstacle {} TRAFO'.format(i))
rospy.sleep(0.2) # Wait zzzz*
while(awaitingTrafo_ee): # TRAFO robot position
try: # Get transformation
self.pos_rob, self.pos_rob = self.listener.lookupTransform("/world", "/lwr_7_link", rospy.Time(0))
awaitingTrafo_ee=False
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo('Waiting for Robot TF')
rospy.sleep(0.2) # Wait zzzz*
#while(awaitingTrafo_attr): # Trafo obs2 position
# try:
# self.pos_attr, self.quat_attr = self.listener.lookupTransform("/world", "/attr/base_link", rospy.Time(0))
# awaitingTrafo_attr=False
# except:
# rospy.loginfo("Waiting for Attractor TF")
# rospy.sleep(0.2) # Wait zzzz*
rospy.loginfo("All TF recieved")
self.attractor_recieved = False # Set initial value
# Variables for trajectory prediciton
while not rospy.is_shutdown():
if np.sum(np.abs([self.pos_attr.x, self.pos_attr.y, self.pos_attr.z] ) ) == 0:
# Default pos -- command with angular position in high level controller
rate.sleep()
if self.attractor_recieved: # called only first time
self.zero_vel()
self.attractor_recieved = False
continue
self.attractor_recieved = True # Nonzero attractor recieved
try: # Get transformation
self.pos_rob, self.quat_rob = self.listener.lookupTransform("/world", "/lwr_7_link", rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("No <</lwr_7_link>> recieved")
#continue
x0_lwr7 = np.array([0,0,0])+np.array(self.pos_rob)
obs_roboFrame = copy.deepcopy(self.obs) # TODO -- change, because only reference is created not copy...
for n in range(len(self.obs)): # for all bostacles
try: # Get transformation
self.pos_obs[n], self.quat_obs[n] = self.listener.lookupTransform("/world", "/object_{}/base_link".format(n), rospy.Time(0))
except:# (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("No <<object{}>> recieved".format(n))
#continue
quat_thr = tf.transformations.quaternion_from_euler(self.obs[n].th_r[0],
self.obs[n].th_r[1],
self.obs[n].th_r[2])
quat_thr_mocap = tf.transformations.quaternion_multiply(self.quat_obs[n], quat_thr)
th_r_mocap = tf.transformations.euler_from_quaternion(quat_thr_mocap)
# TODO apply transofrm to x0
obs_roboFrame[n].x0 = self.obs[n].x0 + np.array(self.pos_obs[n]) # Transpose into reference frame
obs_roboFrame[n].th_r = [th_r_mocap[0],
th_r_mocap[1],
th_r_mocap[2]]# Rotate into reference frame
q0 = Quaternion(1, 0, 0, 0) # Unit quaternion for trajectory
x = x0_lwr7 # x for trajectory creating
x_hat = x0_lwr7 # x fro linear DS
obs_roboFrame[0].w = [0,0,0]
obs_roboFrame[0].xd = [0,0,0]
x_attr = np.array([self.pos_attr.x, self.pos_attr.y, self.pos_attr.z]) # attracotr position
# Create obstacles class instances
obs_list = []
for ii in range(self.n_obs):
obs_list.append(
ObstacleClass(
a=obs_roboFrame[ii].a,
p=obs_roboFrame[ii].p,
x0=obs_roboFrame[ii].x0,
th_r=obs_roboFrame[ii].th_r,
sf=obs_roboFrame[ii].sf
) )
obs_list[ii].draw_ellipsoid()
# Get common center
obs_common_section(obs_list)
# Add the basket wall as an obstacle -- it it is not considered in common center and therefore added later (HACK for fast simulation..)
self.obs_basket.center_dyn = copy.deepcopy(self.obs_basket.x0)
obs_list.append(self.obs_basket)
self.obs_convey.center_dyn = copy.deepcopy(self.obs_convey.x0)
obs_list.append(self.obs_convey)
ds_init = linearAttractor_const(x, x0=x_attr)
#print('vel_init', np.sqrt(np.sum(ds_init**2)))
ds_modulated = obs_avoidance_convergence(x, ds_init, obs_list)
#print('vel', np.sqrt(np.sum(ds_modulated**2)))
vel = Twist()
vel.linear = Vector3(ds_modulated[0],ds_modulated[1],ds_modulated[2])
vel.angular = Vector3(0,0,0)
self.pub_vel.publish(vel)
quat = Quaternion(self.quat_attr.x,
self.quat_attr.y,
self.quat_attr.z,
self.quat_attr.w)
pub_orient.publish(quat)
print("Velcontrol <<world>>:", ds_modulated)
showCenter = True
if showCenter:
point = PointStamped()
point.header.frame_id = 'world'
point.header.stamp = rospy.Time.now()
for ii in range(self.n_obs):
if hasattr(obs_list[ii], 'center_dyn'):
point.point = Point(obs_list[ii].center_dyn[0],
obs_list[ii].center_dyn[1],
obs_list[ii].center_dyn[2])
else:
point.point = Point(obs_list[ii].x0[0],
obs_list[ii].x0[1],
obs_list[ii].x0[2])
pub_cent[ii].publish(point)
rate.sleep()
self.shutdown()
def tf_get_pose(from_frame,
to_frame,
tx,
ty,
tz,
qx,
qy,
qz,
qw,
listener,
use_common_time=True,
stamp=None,
pose_or_point=True):
# Construct input to tf transformPose()
if pose_or_point:
from_pose = PoseStamped()
from_pose.header.frame_id = from_frame
from_pose.pose.position.x = tx
from_pose.pose.position.y = ty
from_pose.pose.position.z = tz
from_pose.pose.orientation.x = qx
from_pose.pose.orientation.y = qy
from_pose.pose.orientation.z = qz
from_pose.pose.orientation.w = qw
else:
from_pose = PointStamped()
from_pose.header.frame_id = from_frame
from_pose.point.x = tx
from_pose.point.y = ty
from_pose.point.z = tz
# This is by design of this function. If you dont' do this, will error
# at transformPose or transformPoint, because the wrong timestamp (now())
# is set, instead of the right one (common_time).
if stamp is None:
use_common_time = True
# I think transformPose() is dangerous, because it could alternate
# the frame's orientation in pose.orientation, which we can't use
# in Marker.POINTS, because all POINTS must share the same
# orientation.
# So it's safer to use transformPoint() instead.
'''
from_pt = PointStamped ()
from_pt.header.frame_id = from_frame
from_pt.point.x = tx
from_pt.point.y = ty
from_pt.point.z = tz
'''
#print ('Trying to get tf from %s to %s' % (from_frame, to_frame)
if stamp is not None:
from_pose.header.stamp = stamp
rospy.loginfo ('tf_get_pose(): Waiting for transform from %s to %s at timestamp %d.%d' \
%(from_frame, to_frame, stamp.secs, stamp.nsecs))
# Wait till it's there.
# If it's never there, and you're running between ReFlex and Baxter, check
# your clocks. See this file's header comment. Did you sync your desktop
# clock to Baxter robot's?
# This intermitently doesn't work, for some reason, even when desktop is
# synced to Baxter, and rosrun tf tf_echo spits out time 3 seconds later
# than the one I'm requesting, at same time this script is hanging on
# waitForTransform(). It's not clear why the tf arrived tf_echo but
# doesn't arrive my waitForTransform()!! This is the case even if I wait
# 10 seconds.
# Minutes before this kind of behavior, it would work on the fly. Cause
# of problem unclear.
try:
# This doesn't get killed by Ctrl+C, for some reason
listener.waitForTransform(to_frame, from_frame, stamp,
rospy.Duration(6.0))
except tf.Exception, err:
# This doesn't work if cache is empty. Just pass in None, faster
#common_time = listener.getLatestCommonTime (to_frame, from_frame)
print(
'waitForTransform() did not receive transform for timestamp %d.%d, using latest common time instead'
% (stamp.secs, stamp.nsecs))
stamp = None
use_common_time = True
def execute_cb(self, goal):
rospy.loginfo("Goal received")
success = True
self.nav_goal = goal.target_pose.pose
self.nav_goal_frame = goal.target_pose.header.frame_id
if self.nav_goal_frame is None or self.nav_goal_frame == '':
rospy.logwarn("Goal has no frame id! Using utm by default")
self.nav_goal_frame = 'utm'
goal_point = PointStamped()
goal_point.header.frame_id = self.nav_goal_frame
goal_point.header.stamp = rospy.Time(0)
goal_point.point.x = self.nav_goal.position.x
goal_point.point.y = self.nav_goal.position.y
goal_point.point.z = self.nav_goal.position.z
try:
goal_point_local = self.listener.transformPoint(self.nav_goal_frame, goal_point)
self.nav_goal.position.x = goal_point_local.point.x
self.nav_goal.position.y = goal_point_local.point.y
self.nav_goal.position.z = goal_point_local.point.z
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print ("Not transforming point to world local")
pass
rospy.loginfo('Nav goal in local %s ' % self.nav_goal.position.x)
r = rospy.Rate(11.) # 10hz
counter = 0
while not rospy.is_shutdown() and self.nav_goal is not None:
self.yaw_pid_enable.publish(True)
self.depth_pid_enable.publish(True)
# Preempted
if self._as.is_preempt_requested():
rospy.loginfo('%s: Preempted' % self._action_name)
success = False
self.nav_goal = None
# Stop thrusters
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = 0.
rpm.thruster_back.rpm = 0.
self.rpm_pub.publish(rpm)
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
print('wp depth action planner: stopped thrusters')
self._as.set_preempted(self._result, "Preempted WP action")
return
# Publish feedback
if counter % 5 == 0:
try:
(trans, rot) = self.listener.lookupTransform(self.nav_goal_frame, self.base_frame, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.loginfo("Error with tf:"+str(self.nav_goal_frame) + " to "+str(self.base_frame))
continue
pose_fb = PoseStamped()
pose_fb.header.frame_id = self.nav_goal_frame
pose_fb.pose.position.x = trans[0]
pose_fb.pose.position.y = trans[1]
pose_fb.pose.position.z = trans[2]
self._feedback.base_position = pose_fb
self._feedback.base_position.header.stamp = rospy.get_rostime()
self._as.publish_feedback(self._feedback)
#rospy.loginfo("Sending feedback")
#Compute yaw setpoint.
xdiff = self.nav_goal.position.x - pose_fb.pose.position.x
ydiff = self.nav_goal.position.y - pose_fb.pose.position.y
yaw_setpoint = math.atan2(ydiff,xdiff)
print('xdiff:',xdiff,'ydiff:',ydiff,'yaw_setpoint:',yaw_setpoint)
#compute yaw_error (e.g. for turbo_turn)
yaw_error= -(self.yaw_feedback - yaw_setpoint)
yaw_error= self.angle_wrap(yaw_error) #wrap angle error between -pi and pi
depth_setpoint = self.nav_goal.position.z
self.depth_pub.publish(depth_setpoint)
#self.vbs_pid_enable.publish(False)
#self.vbs_pub.publish(depth_setpoint)
if self.vel_ctrl_flag:
rospy.loginfo_throttle_identical(5, "vel ctrl, no turbo turn")
#with Velocity control
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
# Publish to velocity controller
self.vel_pid_enable.publish(True)
self.vel_pub.publish(self.vel_setpoint)
self.roll_pub.publish(self.roll_setpoint)
#rospy.loginfo("Velocity published")
else:
if self.turbo_turn_flag:
#if turbo turn is included
rospy.loginfo("Yaw error: %f", yaw_error)
if abs(yaw_error) > self.turbo_angle_min and abs(yaw_error) < self.turbo_angle_max:
#turbo turn with large deviations, maximum deviation is 3.0 radians to prevent problems with discontinuities at +/-pi
self.yaw_pid_enable.publish(False)
self.turbo_turn(yaw_error)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(True)
self.vbs_pub.publish(depth_setpoint)
else:
rospy.loginfo_throttle_identical(5,"Normal WP following")
#normal turning if the deviation is small
self.vbs_pid_enable.publish(False)
self.depth_pid_enable.publish(True)
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
self.create_marker(yaw_setpoint,depth_setpoint)
# Thruster forward
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = self.forward_rpm
rpm.thruster_back.rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
else:
#turbo turn not included, no velocity control
rospy.loginfo_throttle_identical(5, "Normal WP following, no turbo turn")
self.yaw_pid_enable.publish(True)
self.yaw_pub.publish(yaw_setpoint)
self.create_marker(yaw_setpoint,depth_setpoint)
# Thruster forward
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = self.forward_rpm
rpm.thruster_back.rpm = self.forward_rpm
self.rpm_pub.publish(rpm)
#rospy.loginfo("Thrusters forward")
counter += 1
r.sleep()
# Stop thruster
self.vel_pid_enable.publish(False)
rpm = DualThrusterRPM()
rpm.thruster_front.rpm = 0.0
rpm.thruster_back.rpm = 0.0
self.rpm_pub.publish(rpm)
#Stop controllers
self.yaw_pid_enable.publish(False)
self.depth_pid_enable.publish(False)
self.vbs_pid_enable.publish(False)
self.vel_pid_enable.publish(False)
rospy.loginfo('%s: Succeeded' % self._action_name)
self._as.set_succeeded(self._result)
def project_to_image_plane(self, point_in_world):
"""Project point from 3D world coordinates to 2D camera image location
Args:
point_in_world (Point): 3D location of a point in the world
Returns:
x (int): x coordinate of target point in image
y (int): y coordinate of target point in image
"""
fx = self.config['camera_info']['focal_length_x']
fy = self.config['camera_info']['focal_length_y']
image_width = self.config['camera_info']['image_width']
image_height = self.config['camera_info']['image_height']
cx = image_width / 2
cy = image_height / 2
# get transform between pose of camera and world frame
trans = None
try:
now = rospy.Time.now()
self.listener.waitForTransform("/world", "/base_link", now,
rospy.Duration(1.0))
tl_point = PointStamped()
tl_point.header.frame_id = "/world"
tl_point.header.stamp = now
tl_point.point.x = point_in_world.x
tl_point.point.y = point_in_world.y
tl_point.point.z = point_in_world.z
tl_point = self.listener.transformPoint("/base_link", tl_point)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as e:
rospy.logerr(
"Failed to find camera to map transform: {}".format(e))
return None
objectPoints = np.array([
tl_point.point.y / tl_point.point.x,
tl_point.point.z / tl_point.point.x, 1.0
])
################################################################################
# Manually tune focal length and camera coordinate for simulator
# for more details about this issue, please reference
# https://discussions.udacity.com/t/focal-length-wrong/358568/22
if fx < 10:
fx = -2580
fy = -2730
cx = 360
cy = 700
objectPoints[2]
################################################################################
# TODO This can be a class member
cameraMatrix = np.array([[fx, 0, 0], [0, fy, 0], [0, 0, 1]])
imagePoints = cameraMatrix.dot(objectPoints)
x = int(round(imagePoints[0]) + cx)
y = int(round(imagePoints[1]) + cy)
if 10 > x > image_width - 10 or 10 > y > image_height - 10:
return None
# rospy.loginfo("objectpoints: x: {}, y: {}, z: {}".format(tl_point.point.x, tl_point.point.y, tl_point.point.z))
return (x, y)
y = goalpoint_in_rear_axel_coords[1]
goalpoint_angle_rad = math.atan(y / x)
distance_to_goalpoint = math.sqrt(x**2 + y**2)
# Calculate the steering control output
# The coordinate system of Lidar and pure_pursuit is counterclockwise
# but the steering direction is clockwise, so we need to negate the output of pure_pursuit
steering_percentage = -pure_pursuit(goalpoint_angle_rad,
distance_to_goalpoint)
# Calculate the torque control output
torque_percentage = torque_calculator.calc_torque_time_based(20)
# torque_percentage = small_velocity_control(laser_data, steering_percentage, 15)
goalpoint_in_lidar_coords = rear_axle_to_lidar_coords(
goalpoint_in_rear_axel_coords)
p_x = goalpoint_in_lidar_coords[0]
p_y = goalpoint_in_lidar_coords[1]
header = Header(stamp=rospy.Time.now(), frame_id="laser")
point = Point(p_x, p_y, 0)
point_stamped = PointStamped(header=header, point=point)
pub_goalpoint.publish(point_stamped)
msg = drive_params()
msg.angle = int(steering_percentage)
msg.velocity = int(torque_percentage)
pub_drive_param.publish(msg)
pub_drive_param.publish(0, 0)
pub_drive_param.unregister()
def place_point(self, point):
msg = PointStamped()
msg.header = self.make_header()
msg.point = Point(point[0], point[1], 0)
self.point_pub.publish(msg)
import math
import rospy
import tf
from geometry_msgs.msg import PoseStamped, PointStamped
if __name__ == '__main__':
point_publisher = rospy.Publisher('/clicked_point', PointStamped, queue_size=10, latch=True)
rospy.init_node('point_publisher', anonymous=False)
seq = 0
x = rospy.get_param(param_name = '~x', default = 1.0)
y = rospy.get_param(param_name = '~y', default = 1.0)
z = rospy.get_param(param_name = '~z', default = 0.0)
n = rospy.get_param(param_name = '~n', default = 3)
while not rospy.is_shutdown() and n > 0:
point = PointStamped()
point.header.frame_id = rospy.get_param("~frame", default="map")
point.header.stamp = rospy.Time.now()
point.header.seq = seq
point.point.x = x
point.point.y = y
point.point.z = z
point_publisher.publish(point)
rospy.loginfo("Point enviado. Coords: %f %f %f"%(point.point.x, point.point.y, point.point.z))
seq = seq + 1
n = n - 1
for i in range(5):
rospy.sleep(rospy.Duration(1.0))
from geometry_msgs.msg import PoseStamped, PointStamped, Quaternion, TwistStamped, Vector3Stamped
from sensor_msgs.msg import NavSatFix, BatteryState
from inspector_software_uav.srv import *
from uav_abstraction_layer.srv import *
from uav_abstraction_layer.msg import *
import PAL_clients as pal
import geo
# Global variables
stop_flag = False
auto_download = False
loaded_mission = False
flight_status = UInt8()
current_pos = PointStamped()
current_gps_pos = NavSatFix()
battery_state = BatteryState()
current_vel = Vector3Stamped()
ual_state = UInt8()
last_ual_state = UInt8()
adl_state = String()
## Json files with mission information
mission_status_file = os.path.expanduser(
"~"
) + '/catkin_ws/src/inspector_software_uav/json_files/mission_status.json'
mission_waypoints_file = os.path.expanduser(
"~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_wps.json'
mission_data_file = os.path.expanduser(
"~") + '/catkin_ws/src/inspector_software_uav/json_files/mission_data.json'
def talker():
rospy.init_node('kio_rtls_PointStamped_talker', anonymous=True)
rospack = rospkg.RosPack() # get an instance of RosPack with the default search paths
rate = rospy.Rate(10) # 10hz
host = rospy.get_param('~host', "127.0.0.1")
port = rospy.get_param('~port', "3016")
device = rospy.get_param('~device', "/dev/ttyACM0")
baudrate = rospy.get_param('~baudrate', "230400")
path = rospy.get_param('~path', rospack.get_path('kio_rtls') + "/bin") # rangingEngine path
map_x = rospy.get_param('~map_x', 0.0)
map_y = rospy.get_param('~map_y', 0.0)
map_z = rospy.get_param('~map_z', 0.0)
cmd_user = "******" % (host, port, device, baudrate) # rangingEngine looks for anchor.config and tag.config at current directory
os.chdir(path)
tags = []
lines = open("tag.config", "r")
for line in lines:
data = line.split()
tags.append(data[0])
lines.close()
rospy.loginfo("Current working directory: " + os.getcwd())
rospy.loginfo("execute command: " + cmd_user)
# change the buffer mode of the subprocess-shell to linebufferd
cmd_stdbuf = "stdbuf -o L"
cmd = cmd_stdbuf + " " + cmd_user
process = subprocess.Popen(cmd.split(" "), stdout=subprocess.PIPE, stderr=subprocess.PIPE, stdin=subprocess.PIPE, bufsize=1)
rospy.loginfo("started process with pid " + str(process.pid))
pubOUTs = {}
for tag in tags:
pubOUTs[tag] = rospy.Publisher("/kio/PointStamped/"+tag+"/out", PointStamped, queue_size=10)
pubERR = rospy.Publisher("/kio/err", String, queue_size=10)
# putting readline of stdout and stderr into non-blocking mode
fd_out = process.stdout.fileno()
fl = fcntl.fcntl(fd_out, fcntl.F_GETFL)
fcntl.fcntl(fd_out, fcntl.F_SETFL, fl | os.O_NONBLOCK)
fd_err = process.stderr.fileno()
fl = fcntl.fcntl(fd_err, fcntl.F_GETFL)
fcntl.fcntl(fd_err, fcntl.F_SETFL, fl | os.O_NONBLOCK)
br = tf.TransformBroadcaster()
while not rospy.is_shutdown():
try:
stdout = process.stdout.readline().lstrip('\r').rstrip('\n')
if len(stdout) > 0:
data = stdout.split(' ')
if data[0] == "003c":
tagId = data[9]
point = PointStamped()
point.point.x = float(data[15])
point.point.y = float(data[16])
point.point.z = float(data[17])
point.header.stamp = rospy.get_rostime()
point.header.frame_id = "kio_frame"
try:
pubOUTs[tagId].publish(point)
except Exception as e:
rospy.logerr("OUT: Error publishing. " + str(e))
pass
br.sendTransform((map_x, map_y, map_z), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "kio_frame", "map")
rospy.loginfo("OUT: " + stdout)
except Exception as e:
if "Errno 11" not in str(e): # Avoid "[Errno 11] Resource temporarily unavailable" error messages when stdout is empty
rospy.logerr("OUT: " + str(e))
pass
try:
stderr = process.stderr.readline().lstrip('\r').rstrip('\n')
if len(stderr) > 0:
pubERR.publish(String(stderr))
rospy.logerr("ERR: " + stderr)
except Exception as e:
if "Errno 11" not in str(e): # Avoid "[Errno 11] Resource temporarily unavailable" error messages when stderr is empty
rospy.logerr("ERR: " + str(e))
pass
rate.sleep()
ret = process.returncode
rospy.signal_shutdown('finish')
sys.exit(ret)
def test_start_up_and_send_random_input(self):
self.output_dir = f'{get_data_dir(os.environ["CODEDIR"])}/test_dir/{get_filename_without_extension(__file__)}'
os.makedirs(self.output_dir, exist_ok=True)
self._config = {
'output_path': self.output_dir,
'world_name': 'default',
'robot_name': 'bebop_real',
'gazebo': False,
'fsm': False,
'control_mapping': False,
'waypoint_indicator': False,
'altitude_control': False,
'mathias_controller_with_KF': False,
'keyboard': False,
'robot_display': False,
}
# spinoff roslaunch
self._ros_process = RosWrapper(launch_file='load_ros.launch',
config=self._config,
visible=True)
# subscribe to command control
publish_topics = [
TopicConfig(topic_name='/fsm/reset', msg_type='Empty'),
TopicConfig(topic_name='/fsm/state', msg_type='String'),
TopicConfig(topic_name='/bebop/cmd_vel', msg_type='Twist'),
TopicConfig(topic_name='/bebop/image_raw', msg_type='Image'),
TopicConfig(topic_name='/mask', msg_type='Image'),
TopicConfig(topic_name='/reference_pose', msg_type='PointStamped'),
TopicConfig(topic_name=
'/bebop/states/common/CommonState/BatteryStateChanged',
msg_type='CommonCommonStateBatteryStateChanged')
]
self.ros_topic = TestPublisherSubscriber(subscribe_topics=[],
publish_topics=publish_topics)
while not rospy.is_shutdown():
# put something random on each topic every second
# reset
if np.random.binomial(1, 0.2) == 1:
self.ros_topic.publishers['/fsm/reset'].publish(Empty())
state = np.random.choice(
['Unknown', 'Running', 'TakenOver', 'Terminated', 'DriveBack'])
self.ros_topic.publishers['/fsm/state'].publish(String(state))
battery = CommonCommonStateBatteryStateChanged()
battery.percent = np.random.randint(0, 100, dtype=np.uint8)
self.ros_topic.publishers[
'/bebop/states/common/CommonState/BatteryStateChanged'].publish(
battery)
self.ros_topic.publishers['/bebop/cmd_vel'].publish(
get_random_twist())
self.ros_topic.publishers['/bebop/image_raw'].publish(
get_random_image((200, 200, 3)))
self.ros_topic.publishers['/mask'].publish(
get_random_image((200, 200, 1)))
# self.ros_topic.publishers['/reference_pose'].publish(get_random_reference_pose())
self.ros_topic.publishers['/reference_pose'].publish(
PointStamped(point=Point(x=2, y=0.5, z=0.5)))
rospy.sleep(0.1)
def __init__(self, name):
Controller.__init__(self, name)
rospy.loginfo('%s: THIS IS MY NAME...', self._name)
self.landing = False
# The child frame for the drone
self._child_frame_id = self._name + '/base_link'
self._parent_frame_id = self._name + '/odom'
# goal height for the bebop
self._goal_height = 1.5 # initial height goal of the bebop
self._goal_yaw = 0.0 # initial yaw-goal for the bebop
# goal PointStamped ('odom'-frame)
# only x & y position
self._goal_point = PointStamped()
# Flag to know whether the goal is reached or not
self.goal_reached = False
# self._my_point = PointStamped() # current PointStamped ('odom'-frame)
self._my_pose = PoseStamped(
) # current PoseStamed of the bebop ('odom'-frame)
self._my_pose.header.frame_id = self._parent_frame_id
self._my_pose.header.stamp = rospy.Time.now()
self._teleop_vel = Twist() # control-signal sent by the
# teleoperation keyboard
####################
# Start subscribers
# start the position callback
rospy.Subscriber(self._name + '/odom', Odometry, self.pos_callback)
# start the command callback from 'bebop_teleop'
rospy.Subscriber(self._name + '/bebop_teleop/command', String,
self.teleop_command_callback)
# start the vellocetiy callback from 'bebop_teleop'
rospy.Subscriber(self._name + '/bebop_teleop/cmd_vel', Twist,
self.teleop_velocity_callback)
rospy.loginfo('%s: child: %s, parent: %s', self._name,
self._child_frame_id, self._parent_frame_id)
self.tfListener.waitForTransform(self._child_frame_id,
self._parent_frame_id, rospy.Time(0),
rospy.Duration(5))
###################
# Start publishers
self._takeoff_pub = rospy.Publisher(self._name + '/takeoff',
Empty,
queue_size=10)
self._land_pub = rospy.Publisher(self._name + '/land',
Empty,
queue_size=10)
self._cmd_vel_pub = rospy.Publisher(self._name + '/cmd_vel',
Twist,
queue_size=10)
self._camera_pub = rospy.Publisher(self._name + '/camera_control',
Twist,
queue_size=10)
# publish a command that lowers the camera angle
camera_angle = Twist()
camera_angle.angular.y = -60
rospy.sleep(0.5)
self._camera_pub.publish(camera_angle)
#################
# # PID Controllers
self.xPID = PID() # pid-controller for x-direction
self.yPID = PID() # pid-controller for y-direction
self.zPID = PID() # pid-controller for z-direction
self.yawPID = yawPID(
) # pid-controller for the yaw (needs a special pid-controller due to the angles)
rospy.loginfo('%s: we have initialized the PIDs', self._name)
def executeBiGrip(self, fold_traj):
tilts = fold_traj.tilts
approach_points = fold_traj.approach_points
grip_points = fold_traj.grip_points
quarter_points = fold_traj.quarter_points
weight_points = fold_traj.weight_points
vertical_points = fold_traj.vertical_points
goal_points = fold_traj.goal_points
rolls = [pi / 2, pi / 2]
#pitches = [pi/4,pi/4]
pitches = [pi / 4, pi / 4]
if not fold_traj.red:
self.executeSmooth(self.last_traj)
self.fold_stance("b")
arms = self.getArms(fold_traj)
#Initially, both are in "approach" mode
(target1, target2) = self.gripPoints(point=approach_points[0],
grip=False,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 4,
preferred_roll=rolls[0],
point2=approach_points[1],
grip2=False,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 4,
preferred_roll2=rolls[1])
#pitches[0] = target1.pitch
#Do the first pickup and nothing with the other arm
self.gripPoints(point=grip_points[0],
grip=False,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 4,
preferred_roll=rolls[0],
grab=True)
StanceUtils.close_gripper(arms[0])
#Do the second pickup, with the first arm interpolated
first_dx = vertical_points[0].point.x - grip_points[0].point.x
first_dy = vertical_points[0].point.y - grip_points[0].point.y
first_dz = vertical_points[0].point.z - grip_points[0].point.z
second_dx = vertical_points[1].point.x - grip_points[1].point.x
second_dy = vertical_points[1].point.y - grip_points[1].point.y
second_dz = vertical_points[1].point.z - grip_points[1].point.z
interp_dx = first_dx - second_dx
interp_dy = first_dy - second_dy
interp_dz = first_dz - second_dz
interp_x = grip_points[0].point.x + interp_dx
interp_y = grip_points[0].point.y + interp_dy
interp_z = grip_points[0].point.z + interp_dz
interp_pt = PointStamped()
interp_pt.point.x = interp_x
interp_pt.point.y = interp_y
interp_pt.point.z = interp_z
interp_pt.header.frame_id = grip_points[0].header.frame_id
#Go to the second approach point
(target2, nothing) = self.gripPoints(point=interp_pt,
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 4,
preferred_roll=rolls[0],
point2=approach_points[1],
arm2=arms[1],
grip2=False,
tilt2=tilts[1],
preferred_pitch2=pi / 4,
preferred_roll2=rolls[1])
#pitches[1] = target2.pitch
self.gripPoints(point=interp_pt,
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 4,
preferred_roll=rolls[0],
point2=grip_points[1],
grip2=False,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 4,
preferred_roll2=rolls[1])
self.gripPoints(point=grip_points[1],
grip=False,
tilt=tilts[1],
arm=arms[1],
preferred_pitch=pi / 4,
preferred_roll=rolls[1],
grab=True)
StanceUtils.close_gripper(arms[1])
else:
arms = self.getArms(fold_traj)
#Bring both to middle
self.gripPoints(point=vertical_points[0],
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 2,
preferred_roll=rolls[0],
point2=vertical_points[1],
grip2=True,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 2,
preferred_roll2=rolls[1]) #FIX pi/4 -> pi/2
#Set first one down
first_dx = goal_points[0].point.x - vertical_points[0].point.x
first_dy = goal_points[0].point.y - vertical_points[0].point.y
first_dz = goal_points[0].point.z - vertical_points[0].point.z
second_dx = goal_points[1].point.x - vertical_points[1].point.x
second_dy = goal_points[1].point.y - vertical_points[1].point.y
second_dz = goal_points[1].point.z - vertical_points[1].point.z
interp_dx = first_dx - second_dx
interp_dy = first_dy - second_dy
interp_dz = first_dz - second_dz
interp_x = goal_points[0].point.x - interp_dx
interp_y = goal_points[0].point.y - interp_dy
interp_z = goal_points[0].point.z - interp_dz
interp_pt = PointStamped()
interp_pt.point.x = interp_x
interp_pt.point.y = interp_y
interp_pt.point.z = interp_z
interp_pt.header.frame_id = grip_points[0].header.frame_id
self.gripPoints(point=interp_pt,
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 2,
preferred_roll=rolls[0],
point2=goal_points[1],
grip2=True,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 2,
preferred_roll2=rolls[1])
#Finally, set last one down
self.gripPoints(point=goal_points[0],
grip=True,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 2,
preferred_roll=rolls[0],
point2=goal_points[1],
grip2=True,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 2,
preferred_roll2=rolls[1])
GripUtils.open_grippers()
goal_points[0].point.z += 0.03
goal_points[1].point.z += 0.03
self.gripPoints(point=goal_points[0],
grip=False,
tilt=tilts[0],
arm=arms[0],
preferred_pitch=pi / 2,
preferred_roll=rolls[0],
point2=goal_points[1],
grip2=False,
tilt2=tilts[1],
arm2=arms[1],
preferred_pitch2=pi / 2,
preferred_roll2=rolls[1])
self.last_traj = fold_traj
def createNewWaypoint():
print("new waypoint!")
# Get the position of the last waypoint.
if len(self.waypoints) > 0:
end = self.waypoints[len(self.waypoints) - 1].point
else:
end = Point()
end.x = self.odom.pose.pose.position.x
end.y = self.odom.pose.pose.position.y
# number of directions
#self.direction_count = 6
# distance from last point in meters
dist = 0.4
# height and width of square to test in meters
size = 0.5
biggerboxmultiplier = 3
entropyDirections = []
entropyDirections = [0 for i in range(0, self.direction_count)]
for i in range(0, self.direction_count):
#print(i)
entropyDirections[i] = grabEntropySquare(
end.x +
math.cos(2 * math.pi / self.direction_count * i) * dist -
size / 2, end.x +
math.cos(2 * math.pi / self.direction_count * i) * dist +
size / 2, end.y +
math.sin(2 * math.pi / self.direction_count * i) * dist -
size / 2, end.y +
math.sin(2 * math.pi / self.direction_count * i) * dist +
size / 2)
#+ grabEntropySquare(end.x + math.cos(2*math.pi/self.direction_count * i)*dist*biggerboxmultiplier - size*biggerboxmultiplier/2, end.x + math.cos(2*math.pi/self.direction_count * i)*dist*biggerboxmultiplier + size*biggerboxmultiplier/2, end.y + math.sin(2*math.pi/self.direction_count * i)*dist*biggerboxmultiplier - size*biggerboxmultiplier/2, end.y + math.sin(2*math.pi/self.direction_count * i)*dist*biggerboxmultiplier + size*biggerboxmultiplier/2)/9*0.1
#TODO: add more boxes
# Initialize a parameter to check if there is an obstacle between the 3rd waypoint and the generated waypoint.
# It is assumed to be true that there is an obstacle between the points.
isObstacle = 1
while isObstacle == 1:
# inc = 0
# #test = 0
print("entropy values:")
for x in entropyDirections:
print(x)
# inc = inc + 1
# Find the direction to place a new waypoint. The square with the highest entropy is chosen.
# The entropy tells the robot where the "highest reward" is.
# TODO: implement tiebreaker?
direction = entropyDirections.index(max(entropyDirections))
#rospy.loginfo("try:"+str(direction))
# Get the number of squares to increase in either direction depending on the calculated direction.
dx = dist * math.cos(
2 * math.pi / self.direction_count * direction)
dy = dist * math.sin(
2 * math.pi / self.direction_count * direction)
# Check for points within one dist
duplicates = 0
for i in range(0, len(self.waypoints)):
#rospy.loginfo("Point " + str(i) +"\nNew point: ("+str(end.x + dx)+","+str(end.y + dy)+")\nOld point: ("+str(self.waypoints.get(i).x)+","+str(self.waypoints.get(i).y)+")")
if math.sqrt(
math.pow(end.x + dx -
self.waypoints[i].point.x, 2) +
math.pow(end.y + dy -
self.waypoints[i].point.y, 2)) < dist:
#rospy.loginfo("Match")
duplicates = duplicates + 1
#else:
#rospy.loginfo("No Match")
# Connect the 3rd point and the theoretical last point.
yMin = min([end.y, end.y + dy])
yMax = max([end.y, end.y + dy])
xMin = min([end.x, end.x + dx])
xMax = max([end.x, end.x + dx])
# Check for bounds errors or if any duplicates exist. If so, restart the sweek by setting the entropy
# sum in that direction to 0. This will prevent that direction from being searched again since the
# algorithm checks for the maximum entropy value.
#print("("+str(direction)+")")
if yMin < self.mapActual.info.origin.position.y + 1 or yMax >= self.mapActual.info.origin.position.y + self.mapActual.info.height * self.mapActual.info.resolution - 1 or xMin < self.mapActual.info.origin.position.x + 1 or xMax >= self.mapActual.info.origin.position.x + self.mapActual.info.width * self.mapActual.info.resolution - 1 or duplicates > 0:
# if duplicates > 0:
# rospy.loginfo("duplicates > 0 (" + str(duplicates) + ")")
entropyDirections[direction] = 0
else:
maximum = 0
for i in range(
int(
round((xMin -
self.mapActual.info.origin.position.x) /
self.mapActual.info.resolution) - 1),
int(
round((xMax -
self.mapActual.info.origin.position.x) /
self.mapActual.info.resolution) + 2)):
for j in range(
int(
round((yMin - self.mapActual.info.origin.
position.y) /
self.mapActual.info.resolution) - 1),
int(
round((yMax - self.mapActual.info.origin.
position.y) /
self.mapActual.info.resolution) +
2)):
regionX = (xMax + xMin) / 2
regionY = (yMax + yMin) / 2
regionPos = PointStamped()
regionPos.header.stamp = rospy.Time()
regionPos.header.frame_id = "map"
regionPos.point = Point(regionX, regionY, 1)
self.region_publisher.publish(regionPos)
if map(i, j) > maximum:
maximum = map(i, j)
if maximum > 70:
entropyDirections[direction] = 0
#rospy.loginfo("maximum > 70 (" + str(maximum) + ")")
else:
self.waypoints = self.waypoints + [
Waypoint(
len(self.waypoints), end.x + dx, end.y + dy,
2 * math.pi / self.direction_count * direction)
]
#printWaypoints()
isObstacle = 0
self.fails = 0
# Track number of fails.
self.fails = self.fails + 1
#rospy.loginfo("Fails: " + str(self.fails))
if self.fails > self.direction_count:
print("fail reset")
reevaluate()
self.fails = 0
isObstacle = 0
def callback(self, msg):
'''callback function for detection result'''
# update delta time
dt = (msg.header.stamp.sec - self.sec) + (msg.header.stamp.nanosec - self.nanosec) / 1e9
self.sec = msg.header.stamp.sec
self.nanosec = msg.header.stamp.nanosec
# get detection
detections = msg.obstacles
num_of_detect = len(detections)
num_of_obstacle = len(self.obstacle_list)
# kalman predict
for obj in self.obstacle_list:
obj.predict(dt)
# transform to global frame
if self.global_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(self.global_frame, msg.header.frame_id, rclpy.time.Time())
msg.header.frame_id = self.global_frame
for i in range(len(detections)):
# transform position (point)
p = PointStamped()
p.point = detections[i].position
detections[i].position = do_transform_point(p, trans).point
# transform velocity (vector3)
v = Vector3Stamped()
v.vector = detections[i].velocity
detections[i].velocity = do_transform_vector3(v, trans).vector
# transform size (vector3)
s = Vector3Stamped()
s.vector = detections[i].size
detections[i].size = do_transform_vector3(s, trans).vector
except LookupException:
self.get_logger().info('fail to get tf from {} to {}'.format(msg.header.frame_id, self.global_frame))
return
# hungarian matching
cost = np.zeros((num_of_obstacle, num_of_detect))
for i in range(num_of_obstacle):
for j in range(num_of_detect):
cost[i, j] = self.obstacle_list[i].distance(detections[j])
obs_ind, det_ind = linear_sum_assignment(cost)
# filter assignment according to cost
new_obs_ind = []
new_det_ind = []
for o, d in zip(obs_ind, det_ind):
if cost[o, d] < self.cost_filter:
new_obs_ind.append(o)
new_det_ind.append(d)
obs_ind = new_obs_ind
det_ind = new_det_ind
# kalman update
for o, d in zip(obs_ind, det_ind):
self.obstacle_list[o].correct(detections[d])
# birth of new detection obstacles and death of disappear obstacle
self.birth(det_ind, num_of_detect, detections)
dead_object_list = self.death(obs_ind, num_of_obstacle)
# apply velocity and height filter
filtered_obstacle_list = []
for obs in self.obstacle_list:
obs_vel = np.linalg.norm(np.array([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z]))
obs_height = obs.msg.position.z
if obs_vel > self.vel_filter[0] and obs_vel < self.vel_filter[1] and obs_height > self.height_filter[0] and obs_height < self.height_filter[1]:
filtered_obstacle_list.append(obs)
# construct ObstacleArray
if self.tracker_obstacle_pub.get_subscription_count() > 0:
obstacle_array = ObstacleArray()
obstacle_array.header = msg.header
track_list = []
for obs in filtered_obstacle_list:
# do not publish obstacles with low speed
track_list.append(obs.msg)
obstacle_array.obstacles = track_list
self.tracker_obstacle_pub.publish(obstacle_array)
# rviz visualization
if self.tracker_marker_pub.get_subscription_count() > 0:
marker_array = MarkerArray()
marker_list = []
# add current active obstacles
for obs in filtered_obstacle_list:
obstacle_uuid = uuid.UUID(bytes=bytes(obs.msg.uuid.uuid))
(r, g, b) = colorsys.hsv_to_rgb(obstacle_uuid.int % 360 / 360., 1., 1.) # encode id with rgb color
# make a cube
marker = Marker()
marker.header = msg.header
marker.ns = str(obstacle_uuid)
marker.id = 0
marker.type = 1 # CUBE
marker.action = 0
marker.color.a = 0.5
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.pose.position = obs.msg.position
angle = np.arctan2(obs.msg.velocity.y, obs.msg.velocity.x)
marker.pose.orientation.z = np.float(np.sin(angle / 2))
marker.pose.orientation.w = np.float(np.cos(angle / 2))
marker.scale = obs.msg.size
marker_list.append(marker)
# make an arrow
arrow = Marker()
arrow.header = msg.header
arrow.ns = str(obstacle_uuid)
arrow.id = 1
arrow.type = 0
arrow.action = 0
arrow.color.a = 1.0
arrow.color.r = r
arrow.color.g = g
arrow.color.b = b
arrow.pose.position = obs.msg.position
arrow.pose.orientation.z = np.float(np.sin(angle / 2))
arrow.pose.orientation.w = np.float(np.cos(angle / 2))
arrow.scale.x = np.linalg.norm([obs.msg.velocity.x, obs.msg.velocity.y, obs.msg.velocity.z])
arrow.scale.y = 0.05
arrow.scale.z = 0.05
marker_list.append(arrow)
# add dead obstacles to delete in rviz
for uuid in dead_object_list:
marker = Marker()
marker.header = msg.header
marker.ns = str(uuid)
marker.id = 0
marker.action = 2 # delete
arrow = Marker()
arrow.header = msg.header
arrow.ns = str(uuid)
arrow.id = 1
arrow.action = 2
marker_list.append(marker)
marker_list.append(arrow)
marker_array.markers = marker_list
self.tracker_marker_pub.publish(marker_array)
def __init__(self):
self.my_id = rospy.get_param("myID", 0)
self.sector_size = rospy.get_param('sector_size', [5.0, 5.0])
self.grid_size = rospy.get_param('grid_size', [10, 10])
self.origin_shifts = rospy.get_param('origin_shifts', [0.0, 0.0])
# initial home sector
self.home_sector = rospy.get_param("home_sector", 1)
# step length along target position vector
self.STEP = rospy.get_param("cmd_step", 0.1)
self.use_joy = rospy.get_param('enable_joystick', False)
self.master_msg = MasterCommand()
self.e_msg = EnemyState()
self.isATTACKER = rospy.get_param("isATTACKER", False)
# flags
self.home_flag = False
self.takeoff_flag = False
self.land_flag = False
self.arm_flag = False
self.disarm_flag = False
self.battle_flag = False
self.iamCaptured = False
self.joy_msg = Joy()
self.joy_msg.axes = [0.0, 0.0, 0.0]
# step size multiplies joystick input
self.joy_FACTOR = rospy.get_param('joystick_factor', 2.0)
# Fence params
self.fence_x_min = rospy.get_param('fence_min_x', 0.0)
self.fence_x_max = rospy.get_param('fence_max_x', 5.0)
self.fence_y_min = rospy.get_param('fence_min_y', 0.0)
self.fence_y_max = rospy.get_param('fence_max_y', 5.0)
# current position of this agent
x, y = self.sector2enu(self.home_sector)
#print "x/y", x, "/", y
self.current_pos = PoseStamped()
self.current_pos.header.stamp = rospy.Time.now()
self.current_pos.pose.position.x = x
self.current_pos.pose.position.y = y
self.current_pos.pose.position.z = 0.0
# current d_velocity
self.current_vel = PointStamped()
self.current_vel.header.stamp = rospy.Time.now()
self.current_vel.point.x = 0.0
self.current_vel.point.y = 0.0
self.current_vel.point.z = 0.0
self.joy_msg = Joy()
self.joy_msg.axes = [0.0, 0.0, 0.0]
# commanded Position
self.cmd_pos = PoseStamped()
self.cmd_pos.pose.position.x = self.current_pos.pose.position.x
self.cmd_pos.pose.position.z = self.current_pos.pose.position.y
# Commanded d_velocity
self.cmd_vel = PointStamped()
self.cmd_vel.point.x = self.current_vel.point.x = 0.0
self.cmd_vel.point.y = self.current_vel.point.y = 0.0
def movement_detection_node():
rospy.init_node("movement_detection_node")
global times, x, y, z, t, xRaw, yRaw, zRaw, xV_tmp, yV_tmp, zV_tmp, tV_tmp, outlier_dis, movement_recording, movement_start, movement_end, count, invalid_movement, outliers_count, num_points_std, std_threshold, num_outliers
global marker, vis_human_pub
rospy.loginfo("Ready to record NEW movement")
smooth_flag = rospy.get_param("movement_detection_node/smooth", False)
filter_flag = rospy.get_param("movement_detection_node/filter", False)
num_points_std = rospy.get_param("movement_detection_node/num_points_std", 24)
std_threshold = rospy.get_param("movement_detection_node/std_threshold", 0.01)
num_outliers = rospy.get_param("movement_detection_node/num_outliers", 10)
outlier_dis = rospy.get_param("movement_detection_node/outlier_dis", 0.1)
smooth_service_name = rospy.get_param("movement_detection_node/smooth_service_name", "trajectory_smoothing")
filter_service_name = rospy.get_param("movement_detection_node/filter_service_name", "static_points_filtering")
marker = Marker()
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time.now()
marker.action = marker.ADD
marker.type = marker.LINE_STRIP
marker.pose.position.x = 0
marker.pose.position.y = 0
marker.pose.position.z = 0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.pose.orientation.w = 1
marker.scale.x = 0.01
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.lifetime = rospy.Duration(100)
marker_bezier = Marker()
marker_bezier.header.frame_id = "base_link"
marker_bezier.header.stamp = rospy.Time.now()
marker_bezier.action = marker_bezier.ADD
marker_bezier.type = marker_bezier.LINE_STRIP
marker_bezier.pose.position.x = 0
marker_bezier.pose.position.y = 0
marker_bezier.pose.position.z = 0
marker_bezier.pose.orientation.x = 0
marker_bezier.pose.orientation.y = 0
marker_bezier.pose.orientation.z = 0
marker_bezier.pose.orientation.w = 1
marker_bezier.scale.x = 0.01
marker_bezier.color.a = 1.0
marker_bezier.color.r = 0.0
marker_bezier.color.g = 1.0
marker_bezier.color.b = 0.0
marker_bezier.lifetime = rospy.Duration(100)
# Use the following subscription if you use the movement detection function
# using Openpose and the custom message Keypoint3d_list
sub = rospy.Subscriber('/transform_topic', Keypoint3d_list, callback, num_points_std)
# Use the following subscription if you use the movement detection function
# using a geometry_msgs/Point msg for each point
# sub = rospy.Subscriber('raw_points', Point, callback, num_points_std)
pub = rospy.Publisher('/candidate_points', PointStampedArray, queue_size=10)
vis_human_pub = rospy.Publisher('/vis_human_topic', Marker, queue_size=10)
vis_bezier_pub = rospy.Publisher('/vis_bezier_topic', Marker, queue_size=10)
raw_pub = rospy.Publisher('/raw_movement_points', PointStampedArray, queue_size=10)
x_raw, y_raw, z_raw, time_raw = [], [], [], []
msg = PointStampedArray()
while not rospy.is_shutdown():
if (not movement_recording and not invalid_movement):
times = []
timenow = None
for i in xrange(len(x)):
x_raw.append(x[i])
y_raw.append(y[i])
z_raw.append(z[i])
time_raw.append(t[i])
if filter_flag:
try:
rospy.wait_for_service(filter_service_name)
filtering = rospy.ServiceProxy(filter_service_name, Filtering)
resp = filtering(x, y, z, t)
x = resp.x
y = resp.y
z = resp.z
t = resp.t
rospy.loginfo("Filtered the points")
except rospy.ServiceException, e:
rospy.logerr("Cleaning service call failed: %s"%e)
# for i in xrange(len(x)):
# point = PointStamped()
# point.point.x = x[i]
# point.point.y = y[i]
# point.point.z = z[i]
# point.header.stamp = rospy.Time.from_sec(t[i])
# msg.points.append(point)
# raw_pub.publish(msg)
msg.points = []
start_time = rospy.Time.now().to_sec()
if smooth_flag:
try:
rospy.wait_for_service(smooth_service_name)
smoothing = rospy.ServiceProxy(smooth_service_name, Smoothing)
resp = smoothing(x, y, z)
x = resp.x_smooth
y = resp.y_smooth
z = resp.z_smooth
t = np.linspace(t[0], t[-1], len(x))
for i in xrange(len(x)):
point = Point()
point.x = x[i]
point.y = y[i]
point.z = z[i]
marker_bezier.points.append(point)
vis_bezier_pub.publish(marker_bezier)
rospy.loginfo("Smoothed the trajectory")
except rospy.ServiceException, e:
rospy.logerr("Smoothing service call failed: %s"%e)
rospy.loginfo('Bezier time duration: %f secs'%(rospy.Time.now().to_sec() - start_time))
# fig = plt.figure()
# ax = plt.axes()
# ax.scatter(x_raw, y_raw, c='blue', s=20)
# ax.scatter(x, y, c='orange', s=20, label=len(x))
# ax.set_xlabel('x(m)')
# ax.set_ylabel('y(m)')
# ax.grid()
# ax.legend()
# plt.show()
x_raw, y_raw, z_raw = [], [], []
for i in xrange(len(x)):
point = PointStamped()
point.point.x = x[i]
point.point.y = y[i]
point.point.z = z[i]
point.header.stamp = rospy.Time.from_sec(t[i])
msg.points.append(point)
pub.publish(msg)
msg.points = []
movement_recording = True
movement_start = False
movement_end = False
count = 0
x, y, z, t = [], [], [], []
xV_tmp, yV_tmp, zV_tmp, tV_tmp = [], [], [], []
xRaw, yRaw, zRaw = [], [], []
outliers_count = []
rospy.sleep(1)
rospy.loginfo("Ready to record NEW movement")
def tf_update_cb(self, tdat):
# update ball position in real time
# Filter ball outside x = 0 - 3.0m relative to base out
self.p_cb = self.objDetector.p_ball_to_cam
# print self.p_cb
if (self.p_cb == []):
# print "Please wait, the system is detecting the ball"
return
# try:
# self.tf_listener.waitForTransform(TARGET_FRAME, SOURCE_FRAME, rospy.Time(), rospy.Duration(0.5))
# except tf.Exception:
# rospy.loginfo("No frame of /ball from /base received, stop calculation. self.start_calc_flag = False")
# p, q = self.tf_listener.lookupTransform(TARGET_FRAME, SOURCE_FRAME, rospy.Time())
# change to subscribe lookuptf directly and multiply directly
# P_cb = copy.deepcopy(self.objDetector.p_ball_to_cam)
P_cb = np.array([
self.objDetector.p_ball_to_cam[0],
self.objDetector.p_ball_to_cam[1],
self.objDetector.p_ball_to_cam[2],
self.objDetector.p_ball_to_cam[3]
])
timestamp = P_cb[3]
P_cb[3] = 1
P_cb = P_cb.reshape((4, 1))
p_sb = np.dot(self.T_sc, P_cb)
self.tf_bc.sendTransform((p_sb[0][0], p_sb[1][0], p_sb[2][0]),
[0, 0, 0, 1], rospy.Time.now(), "ball",
"base")
pos = PointStamped()
pos.header.stamp = timestamp
# pos.point.x = p[0]
# pos.point.y = p[1]
# pos.point.z = p[2]
# print "1: ", p_sb[0][0]
# print "2: ", p_sb[1][0]
# print "3: ", p_sb[2][0]
pos.point.x = p_sb[0][0]
pos.point.y = p_sb[1][0]
pos.point.z = p_sb[2][0]
# filter repeated received tf out
if (self.pos_rec[-1].header.stamp != pos.header.stamp) and (
self.pos_rec[-1].point.x !=
pos.point.x) and (pos.point.x < 3.5) and (
pos.point.x - self.pos_rec[-1].point.x < 1.2):
self.roll_mat(self.pos_rec)
self.pos_rec[-1] = pos
# choose only a non-repeated pos_rec by comparing between the timestamped of the present and past pos_rec
if (self.last_tf_time != self.pos_rec[0].header.stamp):
# If running_flag is True, start detecting
if self.running_flag:
# check if the ball is being thrown yet
if not self.start_calc_flag:
self.check_start_throw()
self.ball_marker.draw_spheres([0, 0.7, 0, 1],
[0.03, 0.03, 0.03],
self.pos_rec[0].point)
self.ball_marker.draw_line_strips([5.7, 1, 4.7, 1],
[0.01, 0, 0],
self.pos_rec[0].point,
self.pos_rec[1].point)
if self.start_calc_flag:
self.check_stop_throw()
if not self.start_calc_flag:
return
# draw markers
self.ball_marker.draw_spheres([0.7, 0, 0, 1],
[0.03, 0.03, 0.03],
self.pos_rec[0].point)
self.ball_marker.draw_line_strips([1, 0.27, 0.27, 1],
[0.01, 0, 0],
self.pos_rec[0].point,
self.pos_rec[1].point)
self.ball_marker.draw_numtxts([1, 1, 1, 1], 0.03,
self.pos_rec[0].point, 0.03)
# calculate the dropping position based on 2 points
print "##########"
print "pos_rec_listB4: ", self.pos_rec_list
self.pos_rec_list = np.append(self.pos_rec_list,
self.pos_rec[0])
# self.pos_rec_list = (self.pos_rec_list).append(self.pos_rec[0])
print "pos_rec_list: ", self.pos_rec_list
print "##########"
self.drop_point = sawyer_calc.opt_min_proj_calc(
self.pos_rec_list, Z_CENTER)
# average drop point
input_posestamped = PoseStamped()
input_posestamped.pose.position = self.drop_point
input_posestamped.pose.orientation = Quaternion(
0.0392407571798, 0.664506667783, -0.0505321422468,
0.744538483926)
if self.pos_rec_list.shape[0] >= 4:
self.ik_cont.running_flag = True
self.ik_cont.set_goal_from_pose(input_posestamped)
self.drop_point_marker.draw_spheres([0, 0, 0.7, 1],
[0.03, 0.03, 0.03],
self.drop_point)
self.drop_point_marker.draw_numtxts([1, 1, 1, 1], 0.03,
self.drop_point, 0.03)
self.last_tf_time = self.pos_rec[0].header.stamp
action='store',
required=True,
dest='output_filename',
help='image file to write')
args = ap.parse_args()
corner_pos = parse_tuple(args.corner, float)
size = parse_tuple(args.size, float)
res = parse_tuple(args.res, int)
n_samples = res[0] * size[0], res[1] * size[1]
print 'Waiting for service "heightmap"...'
rospy.wait_for_service('heightmap')
query = rospy.ServiceProxy('heightmap', heightmap.srv.Query)
corner = PointStamped()
corner.header.frame_id = args.tf_frame
corner.point.x, corner.point.y = corner_pos
print 'Request sent'
req_time = time.time()
response = query(corner=corner,
x_size=size[0],
y_size=size[1],
x_samples=n_samples[0],
y_samples=n_samples[1])
print 'Got response in', (time.time() - req_time), 'secs'
data = np.array(response.map).reshape(response.y_samples, response.x_samples)
print 'Exporting image: size', data.shape
# PIL expects floats to be in the range 0.0-255.0 (even though they
def node():
global frontiers,mapData,global1,global2,global3,globalmaps
rospy.init_node('filter', anonymous=False)
# fetching all parameters
map_topic= rospy.get_param('~map_topic','/robot_1/map')
threshold= rospy.get_param('~costmap_clearing_threshold',70)
info_radius= rospy.get_param('~info_radius',1.0) #this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
goals_topic= rospy.get_param('~goals_topic','/detected_points')
n_robots = rospy.get_param('~n_robots',1)
rateHz = rospy.get_param('~rate',100)
rate = rospy.Rate(rateHz)
#-------------------------------------------
rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
#---------------------------------------------------------------------------------------------------------------
for i in range(0,n_robots):
globalmaps.append(OccupancyGrid())
for i in range(0,n_robots):
rospy.Subscriber('/robot_'+str(i+1)+'/move_base_node/global_costmap/costmap', OccupancyGrid, globalMap)
#wait if map is not received yet
while (len(mapData.data)<1):
pass
#wait if any of robots' global costmap map is not received yet
for i in range(0,n_robots):
while (len(globalmaps[i].data)<1):
pass
global_frame="/"+mapData.header.frame_id
tfLisn=tf.TransformListener()
for i in range(0,n_robots):
tfLisn.waitForTransform(global_frame[1:], 'robot_'+str(i+1)+'/base_link', rospy.Time(0),rospy.Duration(10.0))
rospy.Subscriber(goals_topic, PointStamped, callback=callBack,callback_args=[tfLisn,global_frame[1:]])
pub = rospy.Publisher('frontiers', Marker, queue_size=10)
pub2 = rospy.Publisher('centroids', Marker, queue_size=10)
filterpub = rospy.Publisher('filtered_points', PointArray, queue_size=10)
rospy.loginfo("the map and global costmaps are received")
# wait if no frontier is received yet
while len(frontiers)<1:
pass
points=Marker()
points_clust=Marker()
#Set the frame ID and timestamp. See the TF tutorials for information on these.
points.header.frame_id= mapData.header.frame_id
points.header.stamp= rospy.Time.now()
points.ns= "markers2"
points.id = 0
points.type = Marker.POINTS
#Set the marker action for latched frontiers. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points.action = Marker.ADD;
points.pose.orientation.w = 1.0
points.scale.x=0.2
points.scale.y=0.2
points.color.r = 255.0/255.0
points.color.g = 255.0/255.0
points.color.b = 0.0/255.0
points.color.a=1;
points.lifetime = rospy.Duration();
p=Point()
p.z = 0;
pp=[]
pl=[]
points_clust.header.frame_id= mapData.header.frame_id
points_clust.header.stamp= rospy.Time.now()
points_clust.ns= "markers3"
points_clust.id = 4
points_clust.type = Marker.POINTS
#Set the marker action for centroids. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
points_clust.action = Marker.ADD;
points_clust.pose.orientation.w = 1.0;
points_clust.scale.x=0.2;
points_clust.scale.y=0.2;
points_clust.color.r = 0.0/255.0
points_clust.color.g = 255.0/255.0
points_clust.color.b = 0.0/255.0
points_clust.color.a=1;
points_clust.lifetime = rospy.Duration();
temppoint=PointStamped()
temppoint.header.frame_id= mapData.header.frame_id
temppoint.header.stamp=rospy.Time(0)
temppoint.point.z=0.0
arraypoints=PointArray()
tempPoint=Point()
tempPoint.z=0.0
#-------------------------------------------------------------------------
#--------------------- Main Loop -------------------------------
#-------------------------------------------------------------------------
while not rospy.is_shutdown():
#-------------------------------------------------------------------------
#Clustering frontier points
centroids=[]
front=copy(frontiers)
if len(front)>1:
ms = MeanShift(bandwidth=0.3)
ms.fit(front)
centroids= ms.cluster_centers_ #centroids array is the centers of each cluster
#if there is only one frontier no need for clustering, i.e. centroids=frontiers
if len(front)==1:
centroids=front
frontiers=copy(centroids)
#-------------------------------------------------------------------------
#clearing old frontiers
z=0
while z<len(centroids):
cond=False
temppoint.point.x=centroids[z][0]
temppoint.point.y=centroids[z][1]
for i in range(0,n_robots):
transformedPoint=tfLisn.transformPoint(globalmaps[i].header.frame_id,temppoint)
x=array([transformedPoint.point.x,transformedPoint.point.y])
cond=(gridValue(globalmaps[i],x)>threshold) or cond
if (cond or (informationGain(mapData,[centroids[z][0],centroids[z][1]],info_radius))<1.0):
centroids=delete(centroids, (z), axis=0)
z=z-1
z+=1
#-------------------------------------------------------------------------
#publishing
arraypoints.points=[]
for i in centroids:
tempPoint.x=i[0]
tempPoint.y=i[1]
arraypoints.points.append(copy(tempPoint))
filterpub.publish(arraypoints)
pp=[]
for q in range(0,len(frontiers)):
p.x=frontiers[q][0]
p.y=frontiers[q][1]
pp.append(copy(p))
points.points=pp
pp=[]
for q in range(0,len(centroids)):
p.x=centroids[q][0]
p.y=centroids[q][1]
pp.append(copy(p))
points_clust.points=pp
pub.publish(points)
pub2.publish(points_clust)
rate.sleep()
#!/usr/bin/env python
import roslib
roslib.load_manifest('stereo_click')
from geometry_msgs.msg import PointStamped
import rospy
import sys
import os
if len(sys.argv) < 3:
print 'Usage: rosrun stereo_click replay_points_3d.py <points_3d_file> <frame_id>'
print '- <points_3d_file> should contain one point per line with x,y,z separated by spaces'
print '- frame_id is e.g. "base_footprint"'
rospy.init_node('replay_points_3d')
points_3d = open(sys.argv[1], 'r').read()
pub = rospy.Publisher('/stereo_points_3d', PointStamped)
frame_id = sys.argv[2]
rospy.sleep(1.0) # Need publisher to initialize
for pt in points_3d.split('\n'):
pt = pt.split()
if len(pt) != 3:
continue
ps = PointStamped()
ps.header.frame_id = frame_id
ps.point.x = float(pt[0])
ps.point.y = float(pt[1])
ps.point.z = float(pt[2])
pub.publish(ps)
def spray_weed_callback(self, data):
time = rospy.Time(0)
all_points = data.points
# Get the sprayer position in the map coordinates
try:
trans, rot = self.tflistener.lookupTransform(
'map', '{}/sprayer'.format(self.robot), data.header.stamp)
except Exception:
return
# First initialise current_y_sprayer = result from tf
# it's going to be updated according to the place it travels to.
# Iterate every detected Weed
for point in data.points:
# If you see any weeds in current crop line
# (crop line has a width of 1 meter thats why <0.5)
if abs(trans[1] - point.y) < 0.5:
# pos of robot
x_sprayer = trans[0]
current_y_sprayer = trans[1]
# Difference in 'x' and 'y' frame
dx = abs(x_sprayer - point.x)
# this creates mirror effect
dy = current_y_sprayer - point.y
# When sprayer moves, sleep for travel time
# self.slep(dy)
if dx < 0.05:
if point not in self.sprayed:
new_point = PointStamped()
new_point.header.frame_id = "map"
new_point.header.stamp = rospy.Time()
new_point.point.x = point.x
new_point.point.y = point.y
p = self.tflistener.transformPoint(
"{}/sprayer".format(self.robot), new_point)
# Initialise request and assign to it the difference in y axis
# between the sprayer and the weed
req = y_axes_diffRequest()
# req has to be the difference from center of sprayer
# and weed detected IN REFERENCE TO THE SPRAYER!
req.y_diff = p.point.y
new_point2 = PointStamped()
new_point2.header.frame_id = "{}/sprayer".format(
self.robot)
new_point2.header.stamp = rospy.Time()
new_point2.point.x = p.point.x
new_point2.point.y = p.point.y
rviz_p = self.tflistener.transformPoint(
"map", new_point2)
# Call service to spray
self.spray_srv(req)
# save the position of the point (visualise in rviz)
real_point = Point32(x_sprayer, rviz_p.point.y,
point.z)
# Save point for visualisation
self.real_sprayed.append(real_point)
# add point in sprayed array
self.sprayed.append(real_point)
# Publish the Sprayed Points in RVIZ
self.sprayed_points_indirect_msg.points = self.real_sprayed2
self.sprayed_points_indirect_msg.header.frame_id = 'map'
self.sprayed_points_indirect_msg.header.stamp = time
self.sprayed_points_indirect_pub.publish(
self.sprayed_points_indirect_msg)
self.sprayed_points_msg.points = self.real_sprayed
self.sprayed_points_msg.header.frame_id = 'map'
self.sprayed_points_msg.header.stamp = time
self.sprayed_points_pub.publish(self.sprayed_points_msg)
# ---------------------------------
# 15.06.2020
# --------------------------
import rospy
# rostopic info /clicked_point =>
from geometry_msgs.msg import PointStamped
if __name__ == '__main__':
try:
datei = open("points2read.txt", "r")
text = datei.readline() # Titelzeile
rospy.loginfo(text)
pub = rospy.Publisher('/next_point', PointStamped, queue_size=10)
rospy.init_node('next_point_talker', anonymous=True)
msg = PointStamped()
msg.header.frame_id = "robot_map"
while True:
# read 3 lines => point-message
ignore_text = datei.readline()
if ignore_text == '': # EOF reached?
rospy.loginfo("No data in file anymore : \n")
break
msg.point.x = float(ignore_text[2:]) # ignore first letters x:
ignore_text = datei.readline()
msg.point.y = float(ignore_text[2:]) # ignore first letters y:
ignore_text = datei.readline()
msg.point.z = float(ignore_text[2:]) # ignore first letters z:
rospy.loginfo("Next Point to be published : \n")
async def main():
# Connect to qtm
connection = await qtm.connect(ip)
# Connection failed?
if connection is None:
print("Failed to connect")
return
else:
print("QTM connected")
# Get 6dof settings from qtm
xml_string = await connection.get_parameters(parameters=["6d"])
body_index = create_body_index(xml_string)
# Name of 6DOF Body in QTM
## wanted_body = "CF1"
def on_packet(packet):
global firstTransform
info, bodies = packet.get_6d_euler()
# Set header for new msg to be published
## msg.header.frame_id = worldFrame
## msg.header.stamp = rospy.Time.now()
## msg.header.seq += 1
## if wanted_body is not None and wanted_body in body_index:
if cf_names is not None and all(
np.isin(cf_names, np.array(list(body_index.keys())))):
# Extract one specific body
## wanted_index = body_index[wanted_body]
## position, rotation = bodies[wanted_index]
# Extract bodies
cf_indices = [body_index[name] for name in cfnames]
position, rotation = bodies[cf_indices]
if firstTransform:
# Initialize kalman filter
rospy.set_param("kalman/initialX", position[0])
rospy.set_param("kalman/initialY", position[1])
rospy.set_param("kalman/initialZ", position[2])
update_params1(
["kalman/initialX", "kalman/initialY", "kalman/initialZ"])
update_params2(
["kalman/initialX", "kalman/initialY", "kalman/initialZ"])
update_params3(
["kalman/initialX", "kalman/initialY", "kalman/initialZ"])
## update_params4(["kalman/initialX", "kalman/initialY", "kalman/initialZ"])
update_params5(
["kalman/initialX", "kalman/initialY", "kalman/initialZ"])
rospy.set_param("kalman/resetEstimation", 1)
update_params1(["kalman/resetEstimation"])
update_params2(["kalman/resetEstimation"])
update_params3(["kalman/resetEstimation"])
## update_params4(["kalman/resetEstimation"])
update_params5(["kalman/resetEstimation"])
firstTransform = False
else:
# PROBABLY CAUSE ERROR WITH 2+ CFs
# Assign position components to msg and publish msg
for i in range(len(msg)):
msg1.point.x = position[0] / 1000
msg1.point.y = position[1] / 1000
msg1.point.z = position[2] / 1000
pub1.publish(msg1)
msg2.point.x = position[0] / 1000
msg2.point.y = position[1] / 1000
msg2.point.z = position[2] / 1000
pub2.publish(msg2)
msg3.point.x = position[0] / 1000
msg3.point.y = position[1] / 1000
msg3.point.z = position[2] / 1000
pub3.publish(msg3)
## msg4.point.x = position[0]/1000
## msg4.point.y = position[1]/1000
## msg4.point.z = position[2]/1000
## pub4.publish(msg4)
msg5.point.x = position[0] / 1000
msg5.point.y = position[1] / 1000
msg5.point.z = position[2] / 1000
pub5.publish(msg5)
## print("{} - Pos: {} - Rot: {}".format(wanted_body, position, rotation))
else:
print("Not all bodies found")
return
# Initialize msg
msg1 = PointStamped()
msg2 = PointStamped()
msg3 = PointStamped()
## msg4 = PointStamped()
msg5 = PointStamped()
msg1.header.seq = 0
msg1.header.stamp = rospy.Time.now()
msg1.header.frame_id = worldFrame
msg2.header.seq = 0
msg2.header.stamp = rospy.Time.now()
msg2.header.frame_id = worldFrame
msg3.header.seq = 0
msg3.header.stamp = rospy.Time.now()
msg3.header.frame_id = worldFrame
## msg4.header.seq = 0
## msg4.header.stamp = rospy.Time.now()
## msg4.header.frame_id = worldFrame
msg5.header.seq = 0
msg5.header.stamp = rospy.Time.now()
msg5.header.frame_id = worldFrame
# Initialize publisher to 'external_position' topic (subscribed by Crazyflie)
## pub = rospy.Publisher("CF1/external_position", PointStamped, queue_size=1)
pub1 = rospy.Publisher("CF1/external_position", PointStamped, queue_size=1)
pub2 = rospy.Publisher("CF2/external_position", PointStamped, queue_size=1)
pub3 = rospy.Publisher("CF3/external_position", PointStamped, queue_size=1)
## pub4 = rospy.Publisher("CF4/external_position", PointStamped, queue_size=1)
pub5 = rospy.Publisher("CF5/external_position", PointStamped, queue_size=1)
# Start streaming frames
await connection.stream_frames(components=["6deuler"], on_packet=on_packet)
# Wait asynchronously 15 seconds
await asyncio.sleep(15)
# Stop streaming
await connection.stream_frames_stop()
def publish_tracked_people(self, now):
"""
Publish markers of tracked people to Rviz and to <people_tracked> topic
"""
people_tracked_msg = PersonArray()
people_tracked_msg.header.stamp = now
people_tracked_msg.header.frame_id = self.publish_people_frame
marker_id = 0
# Make sure we can get the required transform first:
if self.use_scan_header_stamp_for_tfs:
tf_time = now
try:
self.listener.waitForTransform(self.publish_people_frame,
self.fixed_frame, tf_time,
rospy.Duration(1.0))
transform_available = True
except:
transform_available = False
else:
tf_time = rospy.Time(0)
transform_available = self.listener.canTransform(
self.publish_people_frame, self.fixed_frame, tf_time)
marker_id = 0
if not transform_available:
rospy.loginfo(
"Person tracker: tf not avaiable. Not publishing people")
else:
for person in self.objects_tracked:
if person.is_person == True:
if self.publish_occluded or person.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people
# Get position in the <self.publish_people_frame> frame
ps = PointStamped()
ps.header.frame_id = self.fixed_frame
ps.header.stamp = tf_time
ps.point.x = person.pos_x
ps.point.y = person.pos_y
try:
ps = self.listener.transformPoint(
self.publish_people_frame, ps)
except:
rospy.logerr(
"Not publishing people due to no transform from fixed_frame-->publish_people_frame"
)
continue
# publish to people_tracked topic
new_person = Person()
new_person.pose.position.x = ps.point.x
new_person.pose.position.y = ps.point.y
yaw = math.atan2(person.vel_y, person.vel_x)
quaternion = tf.transformations.quaternion_from_euler(
0, 0, yaw)
new_person.pose.orientation.x = quaternion[0]
new_person.pose.orientation.y = quaternion[1]
new_person.pose.orientation.z = quaternion[2]
new_person.pose.orientation.w = quaternion[3]
new_person.id = person.id_num
people_tracked_msg.people.append(new_person)
# publish rviz markers
# Cylinder for body
marker = Marker()
marker.header.frame_id = self.publish_people_frame
marker.header.stamp = now
marker.ns = "People_tracked"
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (
rospy.Duration(3) -
(rospy.get_rostime() - person.last_seen)
).to_sec() / rospy.Duration(3).to_sec() + 0.1
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.id = marker_id
marker_id += 1
marker.type = Marker.CYLINDER
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 1.2
marker.pose.position.z = 0.8
self.marker_pub.publish(marker)
# Sphere for head shape
marker.type = Marker.SPHERE
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.pose.position.z = 1.5
marker.id = marker_id
marker_id += 1
self.marker_pub.publish(marker)
# Text showing person's ID number
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.color.a = 1.0
marker.id = marker_id
marker_id += 1
marker.type = Marker.TEXT_VIEW_FACING
marker.text = str(person.id_num)
marker.scale.z = 0.2
marker.pose.position.z = 1.7
self.marker_pub.publish(marker)
# Arrow pointing in direction they're facing with magnitude proportional to speed
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = (
rospy.Duration(3) -
(rospy.get_rostime() - person.last_seen)
).to_sec() / rospy.Duration(3).to_sec() + 0.1
start_point = Point()
end_point = Point()
start_point.x = marker.pose.position.x
start_point.y = marker.pose.position.y
end_point.x = start_point.x + 0.5 * person.vel_x
end_point.y = start_point.y + 0.5 * person.vel_y
marker.pose.position.x = 0.
marker.pose.position.y = 0.
marker.pose.position.z = 0.1
marker.id = marker_id
marker_id += 1
marker.type = Marker.ARROW
marker.points.append(start_point)
marker.points.append(end_point)
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.2
self.marker_pub.publish(marker)
# <self.confidence_percentile>% confidence bounds of person's position as an ellipse:
cov = person.filtered_state_covariances[0][
0] + person.var_obs # cov_xx == cov_yy == cov
std = cov**(1. / 2.)
gate_dist_euclid = scipy.stats.norm.ppf(
1.0 - (1.0 - self.confidence_percentile) / 2., 0,
std)
marker.pose.position.x = ps.point.x
marker.pose.position.y = ps.point.y
marker.type = Marker.SPHERE
marker.scale.x = 2 * gate_dist_euclid
marker.scale.y = 2 * gate_dist_euclid
marker.scale.z = 0.01
marker.color.r = person.colour[0]
marker.color.g = person.colour[1]
marker.color.b = person.colour[2]
marker.color.a = 0.1
marker.pose.position.z = 0.0
marker.id = marker_id
marker_id += 1
self.marker_pub.publish(marker)
# Clear previously published people markers
for m_id in xrange(marker_id, self.prev_person_marker_id):
marker = Marker()
marker.header.stamp = now
marker.header.frame_id = self.publish_people_frame
marker.ns = "People_tracked"
marker.id = m_id
marker.action = marker.DELETE
self.marker_pub.publish(marker)
self.prev_person_marker_id = marker_id
# Publish people tracked message
self.people_tracked_pub.publish(people_tracked_msg)
def main():
'''
Description: This function captures data on a given object class and
instance by having the robot randomly pick a goal and then
make a route to that goal to take a picture of the object.
Input: None
Return: None
'''
# Set up command line arguments
parser = argparse.ArgumentParser(description='Fetch Data Capture')
parser.add_argument('-c',
'--class',
dest='class_name',
action='store',
help='object class name: mug, stapler, book, etc...')
parser.add_argument('-n',
'--num',
'--number',
dest='class_number',
action='store',
help='object number in data set: mug # 6, 7, 8, etc..')
args = parser.parse_args()
num_necessary_args = 5
if (len(sys.argv) > num_necessary_args):
print "Need to provide command line arguments, use \"-help\" to see examples."
exit()
# Initialize variables
class_name = args.class_name
instance_name = class_name + '_' + str(args.class_number)
rospy.init_node('data_collector', anonymous=True)
num_published_points = 4
sample_min_radius = .6
sample_max_radius = 1
max_spine_height = .386
min_spine_height = 0.00313
spine_offset = 0.0
# set starting_image_index back to 0 when doing a new object
starting_image_index = 0
desired_num_images = 80
# Relevant paths
results_path = "/home/mccallm/catkin_ws/src/lifelong_object_learning/data_captured/"
base_filepath = results_path + class_name + "/" + instance_name
image_filepath = results_path + class_name + "/" + instance_name + "/images/"
circle_image_filepath = results_path + class_name + "/" + instance_name + "/circle_images/"
image_data_filepath = results_path + class_name + "/" + instance_name + "/metadata/"
# Path dir creation
if not os.path.exists(results_path):
os.makedirs(results_path)
if not os.path.exists(base_filepath):
os.makedirs(base_filepath)
if not os.path.exists(image_filepath):
os.makedirs(image_filepath)
if not os.path.exists(circle_image_filepath):
os.makedirs(circle_image_filepath)
if not os.path.exists(image_data_filepath):
os.makedirs(image_data_filepath)
# ROS paths
image_topic = "/head_camera/rgb/image_rect_color"
camera_info_topic = "/head_camera/rgb/camera_info"
map_frame = "/map"
camera_frame = "/head_camera_rgb_optical_frame"
published_point_num_topic = "/object_point_num"
published_point_base_topic = "/object_point"
torso_movement_topic = "/torso_controller/follow_joint_trajectory"
head_movement_topic = "/head_controller/point_head"
node = Node(image_topic, camera_info_topic, camera_frame,
published_point_num_topic, published_point_base_topic,
torso_movement_topic, head_movement_topic,
num_published_points, max_spine_height, min_spine_height,
spine_offset)
count_pub = rospy.Publisher('data_capture_index', String, queue_size=10)
camera_model = PinholeCameraModel()
while node.camera_info is None: # wait for camera info
continue
camera_model.fromCameraInfo(node.camera_info)
while (len(node.points_registered) != node.num_published_points):
rospy.loginfo(str(len(node.points_registered)))
continue
# find center of object
x_center = 0.0
y_center = 0.0
z_center = 0.0
for i in range(node.num_published_points):
x_center += node.published_points[i][0]
y_center += node.published_points[i][1]
z_center += node.published_points[i][2]
x_center = x_center / node.num_published_points
y_center = y_center / node.num_published_points
z_center = z_center / node.num_published_points
rospy.loginfo("x center: " + str(x_center))
rospy.loginfo("y center: " + str(y_center))
rospy.loginfo("z center: " + str(x_center))
# send first goal
goalID = starting_image_index
num_images_captured = starting_image_index
position = node.sample_position(x_center, y_center, sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal")
node.move_base_to(goal_x, goal_y, goal_theta)
g_status = GoalStatus()
image_file_index = starting_image_index
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
if (node.base_client.get_state() == g_status.PREEMPTED) or (
(node.base_client.get_state() == g_status.ABORTED) or
(node.base_client.get_state() == g_status.REJECTED)):
position = node.sample_position(x_center, y_center,
sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
count_pub.publish("New goal ID is " + str(goalID))
rospy.loginfo("New goal ID is " + str(goalID))
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal")
node.move_base_to(goal_x, goal_y, goal_theta)
# when base reaches position, adjust spine and point camera at object
if (node.base_client.get_state() == g_status.SUCCEEDED):
# adjust spine height
spine_height = position[3]
result = node.move_torso(spine_height)
rospy.loginfo("Adjusting spine")
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Spine adjustment succeeded")
# make robot look at object
rospy.sleep(1)
rospy.loginfo("Turning head")
node.look_at("/map", x_center, y_center, z_center)
result = node.point_head_client.wait_for_result()
rospy.loginfo(result)
if result == True:
rospy.loginfo("Head turn succeeded")
rospy.sleep(.1)
# capture and save image
img_cur = node.get_img()
rospy.sleep(.1)
if (img_cur is not None) and (
len(node.points_registered)
== node.num_published_points):
rospy.loginfo("Capturing image")
# find pixel coordinates of points of interest
# tl, tr, bl, br
ref_points = node.published_points
height, width, channels = img_cur.shape
points_to_write = []
for ref_point in ref_points:
ps = PointStamped()
ps.header.frame_id = map_frame
ps.header.stamp = node.tf.getLatestCommonTime(
camera_frame, ps.header.frame_id)
ps.point.x = ref_point[0]
ps.point.y = ref_point[1]
ps.point.z = ref_point[2]
ps_new = node.tf.transformPoint(
camera_frame, ps)
(u, v) = camera_model.project3dToPixel(
(ps_new.point.x, ps_new.point.y,
ps_new.point.z))
points_to_write.append(
[int(round(u)),
int(round(v))])
image_file = image_filepath + instance_name + \
"_" + str(image_file_index) + '.png'
circle_image_file = circle_image_filepath + \
instance_name + "_" + str(image_file_index) + '.png'
text_file = image_data_filepath + instance_name + \
"_" + str(image_file_index) + '.txt'
f = open(text_file, 'w')
f.write(image_file + "\n")
f.write(str(height) + "\t" + str(width) + "\n")
for point in points_to_write:
f.write(str(point[0]) + "\t")
f.write(str(point[1]) + "\n")
f.write(str(goal_x) + "\n")
f.write(str(goal_y) + "\n")
f.write(str(position[3]) + "\n") # spine height
f.close()
circle_img = img_cur.copy()
# visualize
for point in points_to_write:
cv2.circle(circle_img, (point[0], point[1]), 2,
(0, 0, 255), 3)
cv2.imwrite(circle_image_file, circle_img)
cv2.imwrite(image_file, img_cur)
image_file_index += 1
rospy.loginfo("Metadata and Image saved")
rospy.loginfo("Num images captured: " +
str(image_file_index))
# update goal id
goalID += 1
num_images_captured += 1
# Send next position
position = node.sample_position(x_center, y_center,
sample_max_radius,
sample_min_radius)
goal_x = position[0]
goal_y = position[1]
goal_theta = position[2]
# exit if we have tried all positions
if num_images_captured == desired_num_images:
rospy.loginfo("Total images captured: " +
str(image_file_index))
return
# move to next position
count_pub.publish("New goal ID is " + str(goalID))
rospy.loginfo("New goal ID is " + str(goalID))
rospy.loginfo("Goal is " + str(goal_x) + " " + str(goal_y) + " " +
str(goal_theta))
rospy.loginfo("Sending goal...")
node.move_base_to(goal_x, goal_y, goal_theta)
rate.sleep()
def body_pose_cb(self, msg):
rospy.loginfo('%s: ERROR ERROR got 3D body_pose message' %
(self._action_name))
return
if person_id != self.id_to_track:
# not the right person, skip
rospy.loginfo(
"%s: Body Tracker: Tracking ID is %d, skipping pose2D for ID %d",
self._action_name, self.id_to_track, person_id)
return
# position component of the target pose stored as a PointStamped() message.
# create a PointStamped structure to transform via transformPoint
target = PointStamped()
target.header.frame_id = msg.header.frame_id
target.point = msg.pose.position
if target.point.z == 0.0:
rospy.loginfo('%s: skipping blank message' % (self._action_name))
return
rospy.loginfo("%s: Body Tracker: Tracking person at %f, %f, %f",
self._action_name, target.point.x, target.point.y,
target.point.z)
# convert from xyz to pan tilt angles
# TODO: 1) Handle Children - currently assumes everyone is 6 foot tall!
# 2) What happens if robot bows?
if target.point.x < 0.2: # min range of most depth cameras
#rospy.loginfo("%s: Body Tracker: Bad Distance (x) value! %f",
# self._action_name, target.point.x)
return
# math shortcut for approx radians
pan_angle = target.point.y / target.point.x
# OPTION 1: Track actual target height
#person_head_offset_radians = 0.52 # TB2S value - TODO Tune this
#tilt_angle = (target.point.z / target.point.x) + person_head_offset_radians
# OPTION 2: Guess height, based upon distance to person
# FUTURE: combine the two, use "guess" when person is too close?
tilt_angle = 0.4 / target.point.x # SHELDON, CHEST MOUNTED camera
rospy.loginfo("%s: Body Tracker: Pan = %f (%f), Tilt = %f (%f)",
self._action_name, pan_angle, degrees(pan_angle),
tilt_angle, degrees(tilt_angle))
# Send servo commands
if abs(
pan_angle
) > MAX_PAN: # just over 45 degrees - TODO put in actual limits here!
rospy.loginfo("%s: Body Tracker: Pan %f exceeds MAX",
self._action_name, pan_angle)
return
if abs(tilt_angle
) > MAX_TILT: # Limit vertical to assure good tracking
rospy.loginfo("%s: Body Tracker: Tilt %f exceeds MAX",
self._action_name, tilt_angle)
return
head_pan_pub.publish(pan_angle)
head_tilt_pub.publish(-tilt_angle)
import roslib
import rospy
from matplotlib.pyplot import *
from geometry_msgs.msg import WrenchStamped
from geometry_msgs.msg import PointStamped
from std_msgs.msg import Float32
import numpy as np
import math as m
import tf
save_array = True
print_once = True
show_error = False
msg_being_published = False
wrench = WrenchStamped()
R_vect = PointStamped()
true_point = PointStamped()
pub_R = rospy.Publisher("/KF/point_estimation", PointStamped)
pub_theta = rospy.Publisher("/KF/theta", Float32)
pub_t = rospy.Publisher("/KF/true_point", PointStamped)
# --- True point ---
true_point.point.x = 0
true_point.point.y = 0
true_point.point.z = 0.64
# --- Kalman Filter Initialization x = [rx, ry, rz]
r_hat_k = np.matrix([[0], [0], [0.5]])
wStd = 0.01
vStd = 50
start_pt = 0 #13000