def __init__(self):
rospy.init_node('patrol_nav_node', anonymous=False)
rospy.on_shutdown(self.shutdown)
# From launch file get parameters
self.rest_time = rospy.get_param("~rest_time", 5)
self.keep_patrol = rospy.get_param("~keep_patrol", False)
self.random_patrol = rospy.get_param("~random_patrol", False)
self.patrol_type = rospy.get_param("~patrol_type", 0)
self.patrol_loop = rospy.get_param("~patrol_loop", 2)
self.patrol_time = rospy.get_param("~patrol_time", 5)
#set all navigation target pose
self.locations = dict()
self.locations['one'] = Pose(Point(9.7, 13.2, 0.000),
Quaternion(0.000, 0.000, 0.705, 0.708))
self.locations['two'] = Pose(Point(5.3, 12.8, 0.000),
Quaternion(0.000, 0.000, 1.00, 0.000))
self.locations['three'] = Pose(Point(1.4, 11.4, 0.000),
Quaternion(0.000, 0.000, 0.88, -0.46))
self.locations['four'] = Pose(Point(4.3, 8.9, 0.000),
Quaternion(0.000, 0.000, 0.00, 1.00))
self.locations['five'] = Pose(Point(9.0, 7.7, 0.000),
Quaternion(0.000, 0.000, 1.000, 0.000))
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
self.move_base.wait_for_server(rospy.Duration(30))
rospy.loginfo("Connected to move base server")
# Variables to keep track of success rate, running time etc.
loop_cnt = 0
n_goals = 0
n_successes = 0
target_num = 0
running_time = 0
location = ""
start_time = rospy.Time.now()
locations_cnt = len(self.locations)
sequeue = ['one', 'two', 'three', 'four', 'five']
rospy.loginfo("Starting position navigation ")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
#judge if set keep_patrol is true
if self.keep_patrol == False:
if self.patrol_type == 0: #use patrol_loop
if target_num == locations_cnt:
if loop_cnt < self.patrol_loop - 1:
target_num = 0
loop_cnt += 1
rospy.logwarn("Left patrol loop cnt: %d",
self.patrol_loop - loop_cnt)
else:
rospy.logwarn("Now patrol loop over, exit...")
rospy.signal_shutdown('Quit')
break
else:
if self.random_patrol == False:
if target_num == locations_cnt:
target_num = 0
else:
target_num = random.randint(0, locations_cnt - 1)
# Get the next location in the current sequence
location = sequeue[target_num]
rospy.loginfo("Going to: " + str(location))
self.send_goal(location)
# Increment the counters
target_num += 1
n_goals += 1
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(60))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.logerr("ERROR:Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
n_successes += 1
rospy.loginfo("Goal succeeded!")
else:
rospy.logerr("Goal failed with error code:" +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(self.trunc(running_time, 1)) +
" min")
rospy.sleep(self.rest_time)
if self.keep_patrol == False and self.patrol_type == 1: #use patrol_time
if running_time >= self.patrol_time:
rospy.logwarn(
"Now reach patrol_time, back to original position...")
self.send_goal('six')
rospy.signal_shutdown('Quit')
ang_pid = ang_PID()
ang_pid.pos_SetPointx = waypoints[waypoint_index][0]
ang_pid.pos_SetPointy = waypoints[waypoint_index][1]
while not rospy.is_shutdown():
rospy.sleep(0.1)
waypoints = new_waypoints
wpoints = []
while waypoints is None or start_flag == 0:
if waypoints is None:
print "no waypoints"
lock_aux_pointx = 0
waypoints = new_waypoints
for i in range(waypoints.shape[0]):
p = Point()
p.x = waypoints[i][0]
p.y = waypoints[i][1]
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
def gnd_marker_pub(gnd_label, marker_pub, cfg, color = "red"):
length = int(cfg.grid_range[2] - cfg.grid_range[0]) # x direction
width = int(cfg.grid_range[3] - cfg.grid_range[1]) # y direction
gnd_marker = Marker()
gnd_marker.header.frame_id = "/kitti/base_link"
gnd_marker.header.stamp = rospy.Time.now()
gnd_marker.type = gnd_marker.LINE_LIST
gnd_marker.action = gnd_marker.ADD
gnd_marker.scale.x = 0.05
gnd_marker.scale.y = 0.05
gnd_marker.scale.z = 0.05
if(color == "red"):
gnd_marker.color.a = 1.0
gnd_marker.color.r = 1.0
gnd_marker.color.g = 0.0
gnd_marker.color.b = 0.0
if(color == "green"):
gnd_marker.color.a = 1.0
gnd_marker.color.r = 0.0
gnd_marker.color.g = 1.0
gnd_marker.color.b = 0.0
gnd_marker.points = []
# gnd_labels are arranged in reverse order
for j in range(gnd_label.shape[0]):
for i in range(gnd_label.shape[1]):
pt1 = Point()
pt1.x = i + cfg.grid_range[0]
pt1.y = j + cfg.grid_range[1]
pt1.z = gnd_label[j,i]
if j>0 :
pt2 = Point()
pt2.x = i + cfg.grid_range[0]
pt2.y = j-1 +cfg.grid_range[1]
pt2.z = gnd_label[j-1, i]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if i>0 :
pt2 = Point()
pt2.x = i -1 + cfg.grid_range[0]
pt2.y = j + cfg.grid_range[1]
pt2.z = gnd_label[j, i-1]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if j < width-1 :
pt2 = Point()
pt2.x = i + cfg.grid_range[0]
pt2.y = j + 1 + cfg.grid_range[1]
pt2.z = gnd_label[j+1, i]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
if i < length-1 :
pt2 = Point()
pt2.x = i + 1 + cfg.grid_range[0]
pt2.y = j + cfg.grid_range[1]
pt2.z = gnd_label[j, i+1]
gnd_marker.points.append(pt1)
gnd_marker.points.append(pt2)
marker_pub.publish(gnd_marker)
depth_srv.wait_for_service()
rospy.loginfo("Subscribing to recog obj array")
global recognized_array
recognized_array = None
recog_subs = rospy.Subscriber("/recognized_object_array",
RecognizedObjectArray,
callback_recognized_array)
wait_for_recognized_array(depth_srv)
recog_subs.unregister()
rospy.loginfo(
"Got one, getting poses and bounding boxes and saving closest one")
# simulation sweet spot
sweet_spot = Pose(Point(0.3, -0.3, 1.1), Quaternion(
0.0, 0.0, 0.0, 1.0)) # change this for a desired/recognized pose
# real table sweet spot
#sweet_spot = Pose(Point(0.4, -0.2, 0.8), Quaternion(0.0,0.0,0.0,1.0))
closest_id = get_id_of_closest_cluster_to_pose(sweet_spot, tf_listener,
cbbf)
grasp_object_server_goal = GraspObjectGoal()
grasp_object_server_goal.target_id = closest_id # only one cluster, lets suppose its the only one
grasp_object_server_goal.execute_grasp = False # Lets check if we can do something first
rospy.loginfo("Requesting grasp (without execution) for target_id " +
str(closest_id))
grasp_obj_ac.send_goal(grasp_object_server_goal)
grasp_obj_ac.wait_for_result()
grasp_obj_result = grasp_obj_ac.get_result()
def LineFunction(self, curArc, lamb=0, a=1, b=1):
tPos = Point()
tPos.x = (1 - lamb) * a * math.cos(curArc)
tPos.y = (1 - lamb) * b * math.sin(curArc)
return tPos
def callback(self, bearing):
"""This callback occurs whenever the object detection script publishes
to the /detection topic. If the array is [0,0], no object was detected
and we move the UGV along the pre-defined search routine. If the array
is not [0,0], we move the UGV toward the object.
"""
def back_it_up(ve, dist_to_move):
"""This subroutine manually moves the husky forward or backward
a fixed amount at a fixed velocity by bypassing the move_base
package and sending a signal directly to the wheels.
This subroutine is likely to be replaced by:
file:
robot_mv_cmds
subroutine: move_UGV_vel(lv,av,dist_to_move)
"""
sleep_time = 0.1
time_to_move = abs(dist_to_move / ve)
twist = Twist()
twist.linear.x = ve
self.ct_move = 0
while self.ct_move * sleep_time < time_to_move:
self.twi_pub.publish(twist)
self.ct_move = self.ct_move + 1
rospy.sleep(sleep_time)
return None
def rot_cmd(ve, dist_to_move):
"""This subroutine manually rotates the husky along the z-axis a
fixed amount at a fixed velocity by bypassing the move_base package
and sending a signal directly to the wheels.
This subroutine is likely to be replaced by:
file:
robot_mv_cmds
subroutine: move_UGV_vel(lv,av,dist_to_move)
"""
sleep_time = 0.1
time_to_move = abs(dist_to_move / ve)
twist = Twist()
twist.angular.z = ve
self.ct_move = 0
while self.ct_move * sleep_time < time_to_move:
self.twi_pub.publish(twist)
self.ct_move = self.ct_move + 1
rospy.sleep(sleep_time)
return None
if bearing.data[0] == 0:
"""If /detection topic is publishing [0,0] then we do not see an
object. Move along the predefined path.
"""
try:
self.state = self.move_base.get_state()
except:
pass
self.noise_counter = self.noise_counter + 1
rospy.logdebug("I see no object!")
if self.state == 3:
# If move_base is registering 'SUCCEEDED' move to next waypoint
self.current_waypoint = self.current_waypoint + 1
location = self.waypoint_name[self.current_waypoint]
self.goal.target_pose.pose = self.locations[location]
self.goal.target_pose.header.frame_id = 'odom'
rospy.loginfo("Going to: " + str(location))
self.move_base.send_goal(self.goal)
print "Going to: "
print self.goal
self.stall_counter = 0
self.detect_counter = 0
rospy.sleep(2)
else:
# If not registering 'SUCCEEDED', increment stall_counter
self.stall_counter = self.stall_counter + 1
if self.stall_counter > self.stalled_threshold:
# If stall_counter is too high, enter stuck routine.
# Ideally this will be replaced with a catch routine in
# husky_navigation package.
rospy.loginfo("We are stuck!")
rospy.loginfo("Applying catch routine.")
self.move_base.cancel_goal()
rospy.sleep(0.1)
back_it_up(-0.25, 0.5)
rospy.sleep(0.1)
rot_cmd(0.25, 0.25)
rospy.loginfo("Resending goal.")
print "Resending goal: "
print self.goal
self.move_base.send_goal(self.goal)
rospy.sleep(1)
self.ct_move = 0
self.stall_counter = 0
if self.noise_counter > 5:
# Check if we have gotten more blank scans than allowed. Reset
# the detection counter.
self.detect_counter = 0
else:
"""If /detection is not publishing [0,0], we found an object! To
remove noisy signals, we make sure we find 10 valid scans.
"""
if self.detect_counter > 10:
# Check number of valid scans to identify the object.
rospy.logdebug("Object identified.")
# move_base doesn't like targets that are too far away. If the
# range is too far away, fix the range to 10m.
if bearing.data[1] > 10:
bear = 10
else:
bear = bearing.data[1]
# If the UGV is within 3m of the target, set the goal to 0
if bear < 3:
bear = 0
# Create target location in the local reference frame from the
# /detection angle and range
x = bear * np.cos(bearing.data[0]) - 1
y = bear * np.sin(bearing.data[0]) - 1
self.goal.target_pose.header.frame_id = 'base_link'
q = tf.transformations.quaternion_from_euler(
0, 0, bearing.data[0])
if abs(bearing.data[0]) > 1.57:
self.goal.target_pose.pose = Pose(
Point(x * 0.0, y * 0, 0),
Quaternion(q[0], q[1], q[2], q[3]))
rospy.loginfo("Going to (x,y,yaw): (%f,%f,%f)", x * 0.1,
y * 0.1, bearing.data[0])
else:
self.goal.target_pose.pose = Pose(
Point(x, y, 0), Quaternion(q[0], q[1], q[2], q[3]))
rospy.loginfo("Going to (x,y,yaw): (%f,%f,%f)", x, y,
bearing.data[0])
self.goal.target_pose.header.stamp = rospy.Time.now()
self.move_base.send_goal(self.goal)
try:
self.state = self.move_base.get_state()
except:
pass
self.detect_counter = self.detect_counter + 1
if bear < 3:
rospy.set_param('smach_state', 'atBoard')
rospy.signal_shutdown('We are close enough to the object!')
rospy.sleep(2)
rospy.loginfo("State:" + str(self.state))
else:
self.detect_counter = self.detect_counter + 1
self.noise_counter = 0
def pr2_mover(object_list):
# TODO: Initialize variables
test_scene_num = Int32()
test_scene_num.data = 1
dict_list = []
labels = []
centroids = []
for object in object_list:
# TODO: Get the PointCloud for a given object and obtain it's centroid
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroid = np.mean(points_arr, axis=0)
centroids.append([
np.asscalar(centroid[0]),
np.asscalar(centroid[1]),
np.asscalar(centroid[2])
])
# TODO: Get/Read parameters
drop_box_param = rospy.get_param('/dropbox')
object_list_param = rospy.get_param('/object_list')
for idx, dropbox in enumerate(drop_box_param):
if dropbox["group"] == 'red':
red_box_position = dropbox["position"]
elif dropbox["group"] == 'green':
green_box_position = dropbox["position"]
# TODO: Parse parameters into individual variables
for object_param in object_list_param:
object_group = object_param['group']
# TODO: Rotate PR2 in place to capture side tables for the collision map
object_name = String()
object_name.data = object_param['name']
# TODO: Create 'place_pose' for the object
# TODO: Assign the arm to be used for pick_place
arm_name = String()
place_pose = Pose()
place_pose.position = Point()
if object_group == 'red':
arm_name.data = 'left'
place_pose.position.x = red_box_position[0]
place_pose.position.y = red_box_position[1]
place_pose.position.z = red_box_position[2]
elif object_group == 'green':
arm_name.data = 'right'
place_pose.position.x = green_box_position[0]
place_pose.position.y = green_box_position[1]
place_pose.position.z = green_box_position[2]
pick_pose = Pose()
pick_pose.position = Point()
# TODO: Loop through the pick list
for idx, label in enumerate(labels):
print(label, object_name.data)
if label == object_name.data:
pick_pose.position.x = centroids[idx][0]
pick_pose.position.y = centroids[idx][1]
pick_pose.position.z = centroids[idx][2]
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
# Populate various ROS messages
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
send_to_yaml('output_1.yaml', dict_list)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM,
PICK_POSE, PLACE_POSE)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def get_lsq_triangulation_error(self, observation, epsilon=0.5):
# Get the camera intrinsics of both cameras
P1 = self.get_cam_intrinsic()
P2 = self.get_neighbor_cam_intrinsic()
# Convert world coordinates to local camera coordinates for both cameras
# We get the camera extrinsics as follows
trans, rot = self.listener.lookupTransform(
self.rotors_machine_name + '/xtion_rgb_optical_frame', 'world',
rospy.Time(0)) #target to source frame
(r, p, y) = tf.transformations.euler_from_quaternion(rot)
H1 = tf.transformations.euler_matrix(r, p, y, axes='sxyz')
H1[0:3, 3] = trans
print(
str(self.env_id) + ' ' + str(self.rotors_machine_name) + ':' +
str(trans))
trans, rot = self.listener.lookupTransform(
self.rotors_neighbor_name + '/xtion_rgb_optical_frame', 'world',
rospy.Time(0)) #target to source frame
(r, p, y) = tf.transformations.euler_from_quaternion(rot)
H2 = tf.transformations.euler_matrix(r, p, y, axes='sxyz')
H2[0:3, 3] = trans
print(
str(self.env_id) + ' ' + str(self.rotors_neighbor_name) + ':' +
str(trans))
#Concatenate Intrinsic Matrices
intrinsic = np.array((P1[0:3, 0:3], P2[0:3, 0:3]))
#Concatenate Extrinsic Matrices
extrinsic = np.array((H1, H2))
joints_gt = self.get_joints_gt()
error = 0
self.triangulated_root = PoseArray()
for k, v in dict_joints.items():
p2d1 = self.joints[dict_joints[k], 0:2]
p2d2 = self.joints_neighbor[dict_joints[k], 0:2]
points_2d = np.array((p2d1, p2d2))
# Solve system of equations using least squares to estimate person position in robot 1 camera frame
estimate_root = self.lstsq_triangulation(intrinsic, extrinsic,
points_2d, 2)
es_root_cam = Point()
es_root_cam.x = estimate_root[0]
es_root_cam.y = estimate_root[1]
es_root_cam.z = estimate_root[2]
err_cov = PoseWithCovarianceStamped()
joints = 0
if v > 4: #because head, eyes and ears lead to high error for the actor
# if v==5 or v == 6:
p = Pose()
p.position = es_root_cam
self.triangulated_root.poses.append(p)
gt = joints_gt.poses[alpha_to_gt_joints[k]].position
error += (self.get_distance_from_point(gt, es_root_cam))
err_cov.pose.pose.position = es_root_cam
err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2
err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2
err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2
err_cov.header.stamp = rospy.Time()
err_cov.header.frame_id = 'world'
self.triangulated_cov_pub[alpha_to_gt_joints[k]].publish(
err_cov)
joints += 1
# Publish all estimates
self.triangulated_root.header.stamp = rospy.Time()
self.triangulated_root.header.frame_id = 'world'
self.triangulated_pub.publish(self.triangulated_root)
is_in_desired_pos = False
error = error / joints
if error <= epsilon:
# error = 0.4*error/epsilon
is_in_desired_pos = True
# else:
# error = 0.4
return [is_in_desired_pos, error]
def node():
global frontiers, mapData, global1, global2, global3, globalmaps, litraIndx, n_robots, namespace_init_count
rospy.init_node('filter', anonymous=False)
#rospy.loginfo('filter and send: I am here')
# fetching all parameters
map_topic = rospy.get_param('~map_topic', '/map')
threshold = rospy.get_param('~costmap_clearing_threshold', 70)
# this can be smaller than the laser scanner range, >> smaller >>less computation time>> too small is not good, info gain won't be accurate
info_radius = rospy.get_param('~info_radius', 1.0)
goals_topic = rospy.get_param('~goals_topic', '/detected_points')
n_robots = rospy.get_param('~n_robots', 1)
namespace = rospy.get_param('~namespace', '')
namespace_init_count = rospy.get_param('namespace_init_count', 1)
rateHz = rospy.get_param('~rate', 100)
global_costmap_topic = rospy.get_param(
'~global_costmap_topic', '/move_base/global_costmap/costmap'
) #Initially /move_base_node/global_costmap/costmap
robot_frame = rospy.get_param('~robot_frame', 'base_link')
litraIndx = len(namespace)
rate = rospy.Rate(rateHz)
# -------------------------------------------
rospy.Subscriber(map_topic, OccupancyGrid, mapCallBack)
# ---------------------------------------------------------------------------------------------------------------
for i in range(0, n_robots):
globalmaps.append(OccupancyGrid())
if len(namespace) > 0:
for i in range(0, n_robots):
rospy.Subscriber(
namespace + str(i + namespace_init_count) +
global_costmap_topic, OccupancyGrid, globalMap)
elif len(namespace) == 0:
rospy.Subscriber(global_costmap_topic, OccupancyGrid, globalMap)
# wait if map is not received yet
while (len(mapData.data) < 1):
rospy.loginfo('Waiting for the map')
rospy.sleep(0.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):
rospy.loginfo('Waiting for the global costmap')
rospy.sleep(0.1)
pass
global_frame = "/" + mapData.header.frame_id
tfLisn = tf.TransformListener()
if len(namespace) > 0:
for i in range(0, n_robots):
tfLisn.waitForTransform(
global_frame[1:],
namespace + str(i + namespace_init_count) + '/' + robot_frame,
rospy.Time(0), rospy.Duration(10.0))
elif len(namespace) == 0:
tfLisn.waitForTransform(global_frame[1:], '/' + robot_frame,
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
#rospy.loginfo('filter and send: I am here')
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
# -------------------------------------------------------------------------
# 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 * 0.5)) < 0.2):
centroids = delete(centroids, (z), axis=0)
z = z - 1
z += 1
frontiers = copy(centroids) #Change this to after the cleannup
# -------------------------------------------------------------------------
# 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()
def __init__(self):
"""
Initialize parameters and flags
"""
self.continue_execution = True
self.vector_battery_low = False
self.vector_issued_dyn_rsp = False
self.is_sim = rospy.get_param("~sim", False)
self.using_amcl = rospy.get_param("~using_amcl", False)
self.global_frame = rospy.get_param("~global_frame", 'odom')
self.base_frame = rospy.get_param("~base_frame", 'base_link')
self.goal_timeout_sec = rospy.get_param("~goal_timeout_sec", 300)
self.load_waypoints = rospy.get_param("~load_waypoints", False)
self.waypoint_dwell_s = rospy.get_param("~waypoints_dwell_time", 5.0)
self.run_waypoints = rospy.get_param("~run_waypoints", False)
self.marker_array_msg = MarkerArray()
self.max_markers = 100
self._init_markers()
self.marker_array_pub = rospy.Publisher('/vector/waypoints',
MarkerArray,
queue_size=10)
rospack = rospkg.RosPack()
self.goals_filename = rospack.get_path(
'vector_navigation_apps') + "/goals/" + rospy.get_param(
"~goalfile", "vector_goals") + ".txt"
"""
Goal state return values
"""
self.goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
self.waypoints = []
self.present_waypoint = 0
if (True == self.load_waypoints):
goalfile = open(self.goals_filename, 'r')
for line in goalfile:
goal = [float(i) for i in line.strip('\n').split(',')]
pose = Pose(Point(goal[0], goal[1], goal[2]),
Quaternion(goal[3], goal[4], goal[5], goal[6]))
self._append_waypoint_pose(pose)
goalfile.close()
"""
Variables to keep track of success rate, running time,
and distance traveled
"""
self.n_goals = 0
self.n_successes = 0
self.distance_traveled = 0
self.start_time = rospy.get_time()
self.running_time = 0
self.vector_operational_state = 0
initial_mode_req = TRACTOR_REQUEST
"""
Initialize subscribers
"""
rospy.Subscriber("/vector/feedback/battery", Battery,
self._handle_low_aux_power)
rospy.Subscriber("/vector/feedback/status", Status,
self._handle_status)
rospy.Subscriber("/move_base_simple/goal", PoseStamped,
self._simple_goal_cb)
rospy.Subscriber('/vector/teleop/abort_navigation', Bool,
self._shutdown)
rospy.Subscriber('/clicked_point', PointStamped, self._add_waypoint)
rospy.Subscriber('/vector/teleop/record_pose', Bool,
self._add_waypoint_pose)
rospy.Subscriber('/vector/waypoint_cmd', UInt32,
self._process_waypoint_cmd)
self.simple_goal_pub = rospy.Publisher('/vector_move_base/goal',
MoveBaseActionGoal,
queue_size=10)
self.new_goal = MoveBaseActionGoal()
"""
Publishers to manually control the robot (e.g. to stop it) and send gp commands
"""
self.config_cmd = ConfigCmd()
self.cmd_config_cmd_pub = rospy.Publisher('/vector/gp_command',
ConfigCmd,
queue_size=10)
self.cmd_vel_pub = rospy.Publisher('/vector/teleop/cmd_vel',
Twist,
queue_size=10)
if (False == self.is_sim):
if (False == self._goto_mode_and_indicate(initial_mode_req)):
rospy.logerr("Could not set operational state")
rospy.logerr("Platform did not respond")
self._shutdown()
return
"""
Get the initial pose from the user
"""
if (True == self.using_amcl):
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
my_cmd = Twist()
my_cmd.angular.z = 1.0
start_time = rospy.get_time()
r = rospy.Rate(10)
while (rospy.get_time() - start_time) < 5.0:
self.cmd_vel_pub.publish(my_cmd)
r.sleep()
my_cmd = Twist()
my_cmd.angular.z = 0.0
self.cmd_vel_pub.publish(my_cmd)
self.last_pose = self._get_current_pose()
if (None == self.last_pose):
rospy.logerr('Could not get initial pose!!!! exiting....')
self._shutdown()
return
"""
Subscribe to the move_base action server
"""
self.move_base_client = actionlib.SimpleActionClient(
"move_base_navi", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...move_base_navi")
"""
Wait 60 seconds for the action server to become available
"""
if (self.move_base_client.wait_for_server(rospy.Duration(60))):
rospy.loginfo("Connected to move base server")
else:
rospy.logerr("Could not connect to action server")
self._shutdown()
return
"""
Start the action server
"""
self.action_ = MoveBaseAction()
self.move_base_server = actionlib.SimpleActionServer(
"vector_move_base",
MoveBaseAction,
execute_cb=self._execute_goal,
auto_start=False)
self.move_base_server.register_preempt_callback(self._preempt_cb)
self.move_base_server.start()
rospy.loginfo("Vector move base server started")
self.waypoint_is_executing = False
self._run_waypoints()
def get_lsq_triangulation_error_with_noisy_gt(self,
observation,
epsilon=0.5):
noisy_joints = self.get_noisy_joints()
stamp = noisy_joints.header.stamp
noisy_joints_neighbor = self.get_noisy_joints_neighbor(stamp)
try:
self.joints = np.array(noisy_joints.res).reshape((14, 2))
self.joints_prev = self.joints
except:
self.joints = self.joints_prev
print('Unable to find noisy joints')
try:
self.joints_neighbor = np.array(noisy_joints_neighbor.res).reshape(
(14, 2))
self.joints_neighbor_prev = self.joints_neighbor
except:
self.joints_neighbor = self.joints_neighbor_prev
print('Unable to find noisy joints from neighbor')
# Get the camera intrinsics of both cameras
P1 = self.get_cam_intrinsic()
P2 = self.get_neighbor_cam_intrinsic()
# Convert world coordinates to local camera coordinates for both cameras
# We get the camera extrinsics as follows
try:
trans, rot = self.listener.lookupTransform(
self.rotors_machine_name + '/xtion_rgb_optical_frame', 'world',
stamp) #target to source frame
self.trans_prev = trans
self.rot_prev = rot
except:
trans = self.trans_prev
rot = self.rot_prev
print('Robot rotation unavailable')
(r, p, y) = tf.transformations.euler_from_quaternion(rot)
H1 = tf.transformations.euler_matrix(r, p, y, axes='sxyz')
H1[0:3, 3] = trans
try:
trans, rot = self.listener.lookupTransform(
self.rotors_neighbor_name + '/xtion_rgb_optical_frame',
'world', stamp) #target to source frame
self.trans2_prev = trans
self.rot2_prev = rot
except:
trans = self.trans2_prev
rot = self.rot2_prev
print('Robot neighbor rotation unavailable')
(r, p, y) = tf.transformations.euler_from_quaternion(rot)
H2 = tf.transformations.euler_matrix(r, p, y, axes='sxyz')
H2[0:3, 3] = trans
#Concatenate Intrinsic Matrices
intrinsic = np.array((P1[0:3, 0:3], P2[0:3, 0:3]))
#Concatenate Extrinsic Matrices
extrinsic = np.array((H1, H2))
joints_gt = self.get_joints_gt()
error = 0
self.triangulated_root = PoseArray()
for k in range(len(gt_joints)):
p2d1 = self.joints[k, :]
p2d2 = self.joints_neighbor[k, :]
points_2d = np.array((p2d1, p2d2))
# Solve system of equations using least squares to estimate person position in robot 1 camera frame
estimate_root = self.lstsq_triangulation(intrinsic, extrinsic,
points_2d, 2)
es_root_cam = Point()
es_root_cam.x = estimate_root[0]
es_root_cam.y = estimate_root[1]
es_root_cam.z = estimate_root[2]
err_cov = PoseWithCovarianceStamped()
#if v>4: #because head, eyes and ears lead to high error for the actor
# if v==5 or v == 6:
p = Pose()
p.position = es_root_cam
self.triangulated_root.poses.append(p)
gt = joints_gt.poses[gt_joints[gt_joints_names[k]]].position
error += (self.get_distance_from_point(gt, es_root_cam))
err_cov.pose.pose.position = es_root_cam
err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2
err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2
err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2
err_cov.header.stamp = rospy.Time()
err_cov.header.frame_id = 'world'
self.triangulated_cov_pub[k].publish(err_cov)
# Publish all estimates
self.triangulated_root.header.stamp = rospy.Time()
self.triangulated_root.header.frame_id = 'world'
self.triangulated_pub.publish(self.triangulated_root)
is_in_desired_pos = False
error = error / len(gt_joints)
if error <= epsilon:
# error = 0.4*error/epsilon
is_in_desired_pos = True
# else:
# error = 0.4
return [is_in_desired_pos, error]
y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
print "The value of x and y and theta are",x,y,theta
rospy.init_node("speed_controller")
sub = rospy.Subscriber("/base_pose_ground_truth", Odometry, newOdom)
sub = rospy.Subscriber('/base_scan', LaserScan, histogram)
pub = rospy.Publisher("/cmd_vel", Twist, queue_size = 1)
speed = Twist()
r = rospy.Rate(4)
goal = Point()
goal.x = 4.2
goal.y = 8.8
while not rospy.is_shutdown():
inc_x = goal.x -x
inc_y = goal.y -y
angle_to_goal = atan2(inc_y, inc_x)
print("The angle to Goal and theta", angle_to_goal,theta)
# if abs((idv*9.5)- theta) > 0.1:
# speed.linear.x = 0.0
# speed.angular.z = 0.3
# else:
# speed.linear.x = 0.5
def main():
NavigationStateMachine()
if __name__ == '__main__':
waypoints = {0: MoveBaseGoal(), # Base
1: MoveBaseGoal(), # Goal small
2: MoveBaseGoal(), # Goal medium
3: MoveBaseGoal(), # Goal large
4: MoveBaseGoal(), # Goal x-large
5: MoveBaseGoal(), # Goal xx-large
}
waypoints[0].target_pose.header.frame_id = 'map'
waypoints[0].target_pose.pose = Pose(Point(-0.579, 0.187, 0.000),
Quaternion(*quaternion_from_euler(0, 0, 2.733, 'ryxz')))
waypoints[1].target_pose.header.frame_id = 'map'
waypoints[1].target_pose.pose = Pose(Point(1.139, -0.534, 0.000),
Quaternion(*quaternion_from_euler(0, 0, -0.407, 'ryxz')))
waypoints[2].target_pose.header.frame_id = 'map'
waypoints[2].target_pose.pose = Pose(Point(1.096, -0.674, 0.000),
Quaternion(*quaternion_from_euler(0, 0, -0.444, 'ryxz')))
waypoints[3].target_pose.header.frame_id = 'map'
waypoints[3].target_pose.pose = Pose(Point(1.039, -0.801, 0.000),
Quaternion(*quaternion_from_euler(0, 0, -0.444, 'ryxz')))
waypoints[4].target_pose.header.frame_id = 'map'
#! /usr/bin/env python
# import ros stuff
import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist, Point
from nav_msgs.msg import Odometry
from tf import transformations
import math
# robot state variables
position_ = Point()
yaw_ = 0
# machine state
state_ = 0
# goal
desired_position_ = Point()
desired_position_.x = -3
desired_position_.y = 7
desired_position_.z = 0
# parameters
yaw_precision_ = math.pi / 90 # +/- 2 degree allowed
dist_precision_ = 0.3
# publishers
pub = None
def clbk_odom(msg):
global position_
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Pose, Point, Quaternion
from tf.transformations import quaternion_from_euler
from vortex_msgs.srv import LandmarkPose, LandmarkPoseResponse
# TODO: actual landmark positions should be found by perception
gate_found = True
gate_pose = Pose(Point(10, 2, -2), Quaternion(*quaternion_from_euler(0, 0, 0)))
buoy_found = True
buoy_pose = Pose(Point(-10, 0, -2),
Quaternion(*quaternion_from_euler(0, 0, 0)))
def landmark_cb(req):
"""
checks if gives landmark is found and returns it's Pose.
Pose (and not point) because the objects orientation can be important.
"""
if req.landmark == 'GATE':
return LandmarkPoseResponse(gate_found, gate_pose)
elif req.landmark == 'BUOY':
return LandmarkPoseResponse(buoy_found, buoy_pose)
else:
rospy.logerr('Unknown landmark requested of landmarks service')
def get_point_from_array(vector):
return Point(x=vector[0], y=vector[1], z=vector[2])
def __init__(self):
super(RingHandler, self).__init__()
self.ring_coordinate = Point()
random.seed(datetime.now())
def _convert_position_to_local_frame(self, source_frame, location):
original = PointStamped()
original.header.frame_id = source_frame
original.point = location
pose = self._tf.transformPoint(self._base_link_frame, original)
return Point(x=pose.point.x, y=pose.point.y, z=pose.point.z)
def __init__(self):
"""This initializes the various attributes in the class
A few key tasks are achieved in the initializer function:
1. We load the pre-defined search routine
2. We connect to the move_base server in ROS
3. We start the ROS subscriber callback function registering
the object
4. We initialize counters in the class to be shared by the various
callback routines
These are the possible returns from move_base.get_state() function.
Included for reference:
goal_states: ['PENDING', 'ACTIVE', 'PREEMPTED','SUCCEEDED', 'ABORTED',
'REJECTED','PREEMPTING', 'RECALLING', 'RECALLED', 'LOST']
"""
# Establish ROS node
rospy.init_node('autonomous', anonymous=True) # Name this node
rospy.on_shutdown(self.shutdown) # Enable shutdown in rospy
rospack = rospkg.RosPack() # Find rospackge locations
# Set up the waypoint locations. Poses are defined in the map frame.
self.locations = dict()
self.waypoint_name = dict()
f = open(
rospack.get_path('autonomy') + '/params/pre-defined-path.txt', 'r')
line_counter = 0
with f as openfileobject:
first_line = f.readline()
for line in openfileobject:
nome = [x.strip() for x in line.split(',')]
self.waypoint_name[line_counter] = nome[0]
x = Decimal(nome[1])
y = Decimal(nome[2])
z = Decimal(nome[3])
X = float(nome[4])
Y = float(nome[5])
Z = float(nome[6])
q = tf.transformations.quaternion_from_euler(X, Y, Z)
self.locations[self.waypoint_name[line_counter]] = Pose(
Point(x, y, z), Quaternion(q[0], q[1], q[2], q[3]))
line_counter = line_counter + 1
# Initialize parameters
self.rest_time = 0.1 # Minimum pause at each location
self.stalled_threshold = 50000 # Loops before stall
self.current_waypoint = 0 # Current waypoint
self.stall_counter = 0 # Stall counter
self.detect_counter = 0 # Detection counter
self.noise_counter = 0 # Noise counter
self.state = 3 # move_base current state
# Set up ROS publishers and subscribers
# Publisher to manually control the robot
self.twi_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
self.goal = MoveBaseGoal()
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Subscribe to object detection topic
rospy.Subscriber("/prepare_to_die", Int8, self.cb_inigo, queue_size=1)
rospy.sleep(self.rest_time)
# If move_base is registering 'SUCCEEDED' move to next waypoint
self.current_waypoint = self.current_waypoint + 1
location = self.waypoint_name[self.current_waypoint]
self.goal.target_pose.pose = self.locations[location]
self.goal.target_pose.header.frame_id = 'odom'
rospy.loginfo("Going to: " + str(location))
self.move_base.send_goal(self.goal)
print "Going to: "
print self.goal
self.stall_counter = 0
self.detect_counter = 0
rospy.sleep(self.rest_time * 50)
rospy.Subscriber("/detection",
numpy_msg(Floats),
self.callback,
queue_size=1)
if __name__ == '__main__':
if len(sys.argv) <= 4:
usage()
rospy.init_node("imidiot_i_need_to_see_rotations")
rospy.sleep(0.3)
if len(sys.argv) == 6:
if sys.argv[5] == '-d':
frame_id = sys.argv[1]
roll = radians(float(sys.argv[2]))
pitch = radians(float(sys.argv[3]))
yaw = radians(float(sys.argv[4]))
else:
frame_id = sys.argv[1]
roll = sys.argv[2]
pitch = sys.argv[3]
yaw = sys.argv[4]
quat = quaternion_from_euler(float(roll), float(pitch), float(yaw))
p = PoseStamped()
p.header.frame_id = frame_id
p.pose.position = Point(2.0, 2.0, 2.0)
p.pose.orientation = Quaternion(*quat)
ori_pub = rospy.Publisher('/arrow_for_rotation', PoseStamped, latch=True)
rospy.loginfo("Painting an arrow in arrow_for_rotation ")
rospy.loginfo("Pose looks like: " + str(p))
while not rospy.is_shutdown():
ori_pub.publish(p)
rospy.sleep(0.5)
def pose_stamped_to_point(pose_stamped):
p = Point()
p.x = pose_stamped.pose.position.x
p.y = pose_stamped.pose.position.y
p.z = pose_stamped.pose.position.z
return p
from using_markers.msg import obstacle_status
from geometry_msgs.msg import Point
from visualization_msgs.msg import Marker
global agent_x_now
global agent_y_now
global initiationPoint
global init_flag
global time_constant
global infl_r
global radius_constant
infl_r = 0.5
time_constant = 1/60.
radius_constant = 0.15
init_flag = 0
initiationPoint = Point()
pub_manager = rospy.Publisher('/manager_task',manager_msgs, queue_size=10)
positionNow = rospy.Publisher('/Position',Point,queue_size=10)
topic = 'agent'
publisher = rospy.Publisher(topic, Marker,queue_size = 100)
marker = Marker()
def makeMarker(x,y):
marker.id = 0
marker.header.frame_id = "/base"
marker.ns = "agent"
marker.type = marker.SPHERE
marker.action = marker.ADD
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.2
def callback_odom(self,data):
self.pose = Point()
self.pose = data.pose.pose.position
def publish_lookhead(self, lookahead):
msg = Point()
msg.x, msg.y = lookahead[:2]
msg.z = 0
self.pub_lookahead.publish(msg)
def control_callback(self):
# print(self.pos_balle)
# On initialise une absence d'objectif
self.has_objective = False
self.objective_x,self.objective_y,self.objective_z = 0.,0.,0.
self.balle_target_id = -1
# Si le robot a une balle dans son panier
if self.has_ball:
# print("Has Ball")
# Partie commentée: une seule balle à la fois, on ne peut pas en prendre plusieurs
# # On regarde si il y a une balle tout près
# for i in range(10):
# if test_ball_near(self.pos_robot,self.pos_balle[i],d):
# print("Ball Near")
# self.objective_x,self.objective_y=self.pos_balle[i,0],self.pos_balle[i,1]
# self.has_objective = True
# # S'il y en a une, on va la chercher
# # Si le robot n'a pas de balle tout près à chercher
# if not self.has_objective:
# S'il est à gauche
# if test_goal(self.pos_robot):
# # print("Inside Goal Zone - Stop")
# self.objective_x,self.objective_y = 0,0
# self.objective_z = 2
# self.has_objective = True
if test_lr(self.pos_robot[0]) == "left":
# print("Robot - Left")
self.objective_x,self.objective_y = goal_left_x,goal_left_y
self.objective_z = 1
self.has_objective = True
# Partie commentée car déjà gérée par le champ de potentiel
# if test_goal_zone(self.pos_robot) or self.goal_status: # S'il est dans la zone d'entrée du but, ou s'il est en train d'aller vers le but, il va vers le but
# # print("Moving Toward Goal")
# self.objective_x,self.objective_y = goal_left_x,goal_left_y
# self.objective_z = 1
# self.goal_status = True
# self.has_objective = True
# else: #Sinon, il se dirige vers la zone d'entrée de but
# # print("Moving Toward Goal Entry Zone")
# self.objective_x,self.objective_y = goal_zone_left_x,goal_zone_left_y
# self.objective_z = 1
# self.has_objective = True
# print("Robot - Right")
else:
self.objective_x,self.objective_y = goal_right_x,goal_right_y
self.objective_z = 1
self.has_objective = True
# Partie commentée car déjà gérée par le champ de potentiel
# if test_goal_zone(self.pos_robot) or self.goal_status:
# # print("Moving Toward Goal")
# self.objective_x,self.objective_y = goal_right_x,goal_right_y
# self.objective_z = 1
# self.has_objective = True
# self.goal_status = True
# else:
# # print("Moving Toward Goal Entry Zone")
# self.objective_x,self.objective_y = goal_zone_right_x,goal_zone_right_y
# self.objective_z = 1
# self.has_objective = True
else:
# Si le robot n'a pas de balle, il ne doit pas aller vers le but de toute façon.
self.goal_status = False
# print("No Ball")
# Sortie du goal commentée car déjà gérée par le champ de potentiel
# if test_goal(self.pos_robot):
# # print("Inside Goal")
# if test_lr(self.pos_robot[0]) == "left":
# # print("Going Outside - Left")
# self.objective_x,self.objective_y = goal_zone_left_x,goal_zone_left_y
# self.objective_z = 1
# self.has_objective = True
# else:
# # print("Going Outside - Right")
# self.objective_x,self.objective_y = goal_zone_right_x,goal_zone_right_y
# self.objective_z = 1
# self.has_objective = True
# else:
if self.num_balle > 0:
self.get_ball_target_id()
# print("Target Ball: ",self.balle_target_id)
if self.balle_target_id >= 0:
#Si on a un objectif
self.objective_x,self.objective_y=self.pos_balle[self.balle_target_id][0],self.pos_balle[self.balle_target_id][1]
self.objective_z = 0
self.has_objective = True
# ###Partie commentée car devenue redondante avec le champ de potentiel
# # Si le robot et la balle sont du même côté:
# # print("self.pos_robot[0]: ",self.pos_robot[0])
# # print("self.pos_balle[self.balle_target_id][0]: ",self.pos_balle[self.balle_target_id][0]
# if test_lr(self.pos_robot[0])==test_lr(self.pos_balle[self.balle_target_id][0]):
# # self.get_logger().error("Robot and Ball in the same side")
# # print("Robot and Ball in the same side")
# self.objective_x,self.objective_y=self.pos_balle[self.balle_target_id][0],self.pos_balle[self.balle_target_id][1]
# # self.get_logger().error("self.pos_balle[self.balle_target_id]: ")
# # self.get_logger().error(str(self.pos_balle[self.balle_target_id]))
# self.objective_z = 0
# self.has_objective = True
# # Le robot va chercher la balle
# else:
# # Sinon, le robot va vers le passage dans le mur le plus proche
# # print("Robot and ball in different sides")
# self.objective_x,self.objective_y=get_passage_wall(self.pos_robot)
# self.objective_z = 1
# self.has_objective = True
# Enfin, on envoie la commande du robot
# self.commande_robot=command_objective(self.pos_robot,[self.objective_x,self.objective_y])
objectif = Point()
objectif.x, objectif.y, objectif.z = float(self.objective_x),float(self.objective_y),float(self.objective_z)
if(self.has_objective == False):
objectif.z = float(2.)
self.publisher_.publish(objectif)
def dropped_it(data):
if data.data == 1:
print "Hello we are in DROPPED_IT"
setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
setmodel(ModelState('object',Pose(Point(1.0, 0.0, 0.0005),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world'))
def update(self, x_pos, y_pos, yaw):
global waypoints, waypoint_index, station_keep_flag, stable, lock, lock_aux_pointx, lock_aux_pointy
self.pos_SetPointx = waypoints[waypoint_index][0]
self.pos_SetPointy = waypoints[waypoint_index][1]
# get waypoint index
last_waypoint_index = waypoint_index
if np.sqrt((waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos)) < 5:
if waypoint_index == waypoints.shape[0] - 1:
waypoint_index = waypoint_index
else:
waypoint_index = waypoint_index + 1
if waypoint_index == waypoints.shape[0] - 1 and np.sqrt(
(waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos)) < 5:
station_keep_flag = 1
else:
station_keep_flag = 0
if time.time() - self.wait_start > 10:
self.wait_flag = 0
#print "delta t", time.time() - self.wait_start
pos_error = np.sqrt((self.pos_SetPointx - x_pos) *
(self.pos_SetPointx - x_pos) +
(self.pos_SetPointy - y_pos) *
(self.pos_SetPointy - y_pos))
waypoint_error = np.sqrt((waypoints[waypoint_index][0] - x_pos) *
(waypoints[waypoint_index][0] - x_pos) +
(waypoints[waypoint_index][1] - y_pos) *
(waypoints[waypoint_index][1] - y_pos))
if np.abs(
waypoint_error) < 5 and station_keep_flag == 1 and stable == 1:
print "turn to desired yaw"
# lock yaw
if waypoints[waypoint_index][2] != -10:
error = waypoints[waypoint_index][2] - yaw
else:
error = np.arctan2((waypoints[waypoint_index][1] -
waypoints[waypoint_index - 1][1]),
(waypoints[waypoint_index][0] -
waypoints[waypoint_index - 1][0])) - yaw
else:
error = np.arctan2((self.SetPointy - y_pos),
(self.SetPointx - x_pos)) - yaw
if waypoint_index >= 1:
last_waypointx = waypoints[waypoint_index - 1][0]
last_waypointy = waypoints[waypoint_index - 1][1]
else:
if lock_aux_pointx == 0:
lock_aux_pointx = x_pos
lock_aux_pointy = y_pos
last_waypointx = lock_aux_pointx
last_waypointy = lock_aux_pointy
if np.abs(waypoints[waypoint_index][0] - last_waypointx) < 1:
dist_to_line = np.abs(x_pos - last_waypointx)
tmp_x = last_waypointx
tmp_y = y_pos
dir_vectorx = 0
dir_vectory = 1
else:
line_slope = (waypoints[waypoint_index][1] - last_waypointy) / (
waypoints[waypoint_index][0] - last_waypointx)
line_intercept = waypoints[waypoint_index][
1] - line_slope * waypoints[waypoint_index][0]
dist_to_line = np.abs(line_slope * x_pos + line_intercept -
y_pos) / np.sqrt(line_slope * line_slope + 1)
tmp_x = x_pos + (line_slope / np.sqrt(line_slope * line_slope + 1)
) * dist_to_line
tmp_y = y_pos - dist_to_line
#print line_slope, line_intercept, dist_to_line
if np.abs(line_slope * tmp_x + line_intercept - tmp_y) / np.sqrt(
line_slope * line_slope + 1) > dist_to_line:
tmp_x = x_pos - (line_slope / np.sqrt(line_slope * line_slope +
1)) * dist_to_line
tmp_y = y_pos + dist_to_line
dir_vectorx = 1 / np.sqrt(line_slope * line_slope + 1)
dir_vectory = line_slope / np.sqrt(line_slope * line_slope + 1)
#print dir_vectorx, dir_vectory
#print dist_to_line
proc_dist = np.sqrt(5 * 5 - dist_to_line * dist_to_line)
aux_pointx = tmp_x + dir_vectorx * proc_dist
aux_point3x = tmp_x + 3 * dir_vectorx * proc_dist
aux_pointy = tmp_y + dir_vectory * proc_dist
aux_point3y = tmp_y + 3 * dir_vectory * proc_dist
if np.abs((waypoints[waypoint_index][0] - tmp_x) *
(waypoints[waypoint_index][0] - tmp_x) +
(waypoints[waypoint_index][1] - tmp_y) *
(waypoints[waypoint_index][1] - tmp_y)) < np.abs(
(waypoints[waypoint_index][0] - aux_pointx) *
(waypoints[waypoint_index][0] - aux_pointx) +
(waypoints[waypoint_index][1] - aux_pointy) *
(waypoints[waypoint_index][1] - aux_pointy)):
aux_pointx = tmp_x - dir_vectorx * proc_dist
aux_point3x = tmp_x - 3 * dir_vectorx * proc_dist
aux_pointy = tmp_y - dir_vectory * proc_dist
aux_point3y = tmp_y - 3 * dir_vectory * proc_dist
#print np.sqrt((tmp_x - x_pos)*(tmp_x - x_pos)+(tmp_y - y_pos)*(tmp_y - y_pos))
if np.sqrt((tmp_x - x_pos) * (tmp_x - x_pos) + (tmp_y - y_pos) *
(tmp_y - y_pos)) < 5:
if station_keep_flag == 1 and pos_error < 5:
if lock == 0:
self.SetPointx = aux_point3x
self.SetPointy = aux_point3y
aux_pointx = aux_point3x
aux_pointy = aux_point3y
lock = 1
aux_pointx = self.SetPointx
aux_pointy = self.SetPointy
else:
self.SetPointx = aux_pointx
self.SetPointy = aux_pointy
lock = 0
#print "aux", aux_pointx, aux_pointy
wpoints = []
for i in range(1):
p = Point()
p.x = aux_pointx
p.y = aux_pointy
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 0
marker.color.b = 1.0
self.pub_aux.publish(marker)
else:
lock = 0
if station_keep_flag == 1:
self.SetPointx = waypoints[waypoint_index][0]
self.SetPointy = waypoints[waypoint_index][1]
tmp_x = waypoints[waypoint_index][0]
tmp_y = waypoints[waypoint_index][1]
else:
if (last_waypointx - x_pos) * (
waypoints[waypoint_index][0] -
last_waypointx) + (last_waypointy - y_pos) * (
waypoints[waypoint_index][1] - last_waypointy) > 0:
self.SetPointx = tmp_x
self.SetPointy = tmp_y
else:
self.SetPointx = waypoints[waypoint_index][0]
self.SetPointy = waypoints[waypoint_index][1]
print "tmp", tmp_x, tmp_y
wpoints = []
for i in range(1):
p = Point()
p.x = tmp_x
p.y = tmp_y
p.z = 0
wpoints.append(p)
marker = Marker()
marker.header.frame_id = "/odom"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.pose.orientation.w = 1
marker.points = wpoints
t = rospy.Duration()
marker.lifetime = t
marker.scale.x = 0.4
marker.scale.y = 0.4
marker.scale.z = 0.4
marker.color.a = 1.0
marker.color.r = 0
marker.color.g = 1.0
marker.color.b = 0
self.pub_aux.publish(marker)
wpoints = []
for i in range(1):
p = Point()
p.x = waypoints[waypoint_index][0]
p.y = waypoints[waypoint_index][1]
p.z = 0
wpoints.append(p)
marker2 = Marker()
marker2.header.frame_id = "/odom"
marker2.type = marker.POINTS
marker2.action = marker.ADD
marker2.pose.orientation.w = 1
marker2.points = wpoints
t = rospy.Duration()
marker2.lifetime = t
marker2.scale.x = 0.4
marker2.scale.y = 0.4
marker2.scale.z = 0.4
marker2.color.a = 1.0
marker2.color.r = 0.5
marker2.color.g = 0
marker2.color.b = 0.5
self.pub_tgwp.publish(marker2)
wpoints = []
for i in range(1):
p = Point()
p.x = last_waypointx
p.y = last_waypointy
p.z = 0
wpoints.append(p)
marker3 = Marker()
marker3.header.frame_id = "/odom"
marker3.type = marker.POINTS
marker3.action = marker.ADD
marker3.pose.orientation.w = 1
marker3.points = wpoints
t = rospy.Duration()
marker3.lifetime = t
marker3.scale.x = 0.4
marker3.scale.y = 0.4
marker3.scale.z = 0.4
marker3.color.a = 1.0
marker3.color.r = 0
marker3.color.g = 0.5
marker3.color.b = 0.5
self.pub_lwp.publish(marker3)
'''
#print np.abs(waypoint_error)
if np.abs(waypoint_error) < 5 and waypoints is not None:
if waypoint_index < waypoints.shape[0]-1:
if self.wait_flag == 0 and station_keep_flag == 0:
waypoint_index = waypoint_index + 1
self.wait_flag = 1
self.wait_start = time.time()
print "target waypoint", waypoint_index
#print waypoint_index, waypoints.shape[0]
else:
station_keep_flag = 1
print "station keep in last waypoint"
'''
#print "error", np.arctan((self.pos_SetPointy - y_pos)/(self.pos_SetPointx - x_pos)) - yaw
if np.abs(error) > np.pi:
if error > 0:
error = error - 2 * np.pi
else:
error = error + 2 * np.pi
if station_keep_flag == 1 and np.abs(waypoint_error) > 5:
if np.abs(error) > np.pi / 2:
if error < 0:
error = error + np.pi
else:
error = error - np.pi
#print "error", error
#print "yaw", yaw
#print "target", np.arctan2((self.pos_SetPointy - y_pos), (self.pos_SetPointx - x_pos))
self.current_time = time.time()
delta_time = self.current_time - self.last_time
delta_error = error - self.last_error
if (delta_time >= self.sample_time):
self.PTerm = self.Kp * error
self.ITerm += error * delta_time
if (self.ITerm < -self.windup_guard):
self.ITerm = -self.windup_guard
elif (self.ITerm > self.windup_guard):
self.ITerm = self.windup_guard
self.DTerm = 0.0
if delta_time > 0:
self.DTerm = delta_error / delta_time
self.last_time = self.current_time
self.last_error = error
self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd *
self.DTerm)
def load_gazebo_models():
# Get Models' Path
table_pose = Pose(position=Point(x=0.8, y=0.85, z=0.0))
table_reference_frame = "world"
object_reference_frame = "world"
model_path = rospkg.RosPack().get_path('baxter_grasping_drl') + "/models/"
rack_pose = Pose(position=Point(x=0.9, y=0.87, z=0.0))
rack_reference_frame = "world"
# Load Table SDF
table_xml = ''
with open(model_path + "cafe_table/model.sdf", "r") as table_file:
table_xml = table_file.read().replace('\n', '')
# Load Camera Rack SDF
rack_xml = ''
with open(model_path + "camera_rack/model.sdf", "r") as rack_file:
rack_xml = rack_file.read().replace('\n', '')
# Load objects URDF
xml = {}
pose = {}
for i in xrange(1, 10):
xml["object" + str(i)] = ''
pose["object" +
str(i)] = Pose(position=Point(x=-3.0 + i * 0.1, y=0.0, z=0.0))
# Red models
# for contact sensor obj number needs to correspond with topic name in model urdf
with open(model_path + "block_r/model.urdf", "r") as object1_file:
xml["object1"] = object1_file.read().replace('\n', '')
with open(model_path + "sphere_r/model.urdf", "r") as object2_file:
xml["object2"] = object2_file.read().replace('\n', '')
with open(model_path + "cylinder_r/model.urdf", "r") as object3_file:
xml["object3"] = object3_file.read().replace('\n', '')
# Green models
with open(model_path + "block_g/model.urdf", "r") as object4_file:
xml["object4"] = object4_file.read().replace('\n', '')
with open(model_path + "sphere_g/model.urdf", "r") as object5_file:
xml["object5"] = object5_file.read().replace('\n', '')
with open(model_path + "cylinder_g/model.urdf", "r") as object6_file:
xml["object6"] = object6_file.read().replace('\n', '')
# Blue models
with open(model_path + "block_b/model.urdf", "r") as object7_file:
xml["object7"] = object7_file.read().replace('\n', '')
with open(model_path + "sphere_b/model.urdf", "r") as object8_file:
xml["object8"] = object8_file.read().replace('\n', '')
with open(model_path + "cylinder_b/model.urdf", "r") as object9_file:
xml["object9"] = object9_file.read().replace('\n', '')
# Spawn Table SDF
rospy.wait_for_service('/gazebo/spawn_sdf_model')
try:
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
resp_sdf = spawn_sdf("cafe_table", table_xml, "/", table_pose,
table_reference_frame)
except rospy.ServiceException, e:
rospy.logerr("Spawn SDF service call failed: {0}".format(e))
def process_msg(self, msg):
_exit_speed = self.target_exit_speed #waypoint exit speed in m/s
_distance_threshold = 2 # in m to switch to the next waypoint
waypoint = Point(self.poses[self.waypoint_index][0],
self.poses[self.waypoint_index][1],
self.poses[self.waypoint_index][2])
d = geo.euclidean_distance(msg.position, waypoint)
if self._last_distance is None:
self._last_distance = d
delta_d = self._last_distance - d
self._last_distance = d
print("W[{}] delta:{}m D:{}m".format(self.waypoint_index, delta_d, d))
# Method 2 for threshold to switch waypoint:
# Use the distance from the plane of the waypoint.
# It should be the scalar projection of the ego position vector (from waypoint to position point),
# into the pose unit vector of the waypoint. This is the dot product of both vectors d=(x0*x1)+(y0*y1)+(z0*z1)
_pos_vector = geo.vector_two_points(msg.position, waypoint)
_waypoint_vector = geo.vector_from_RPY(
self.poses[self.waypoint_index][3],
self.poses[self.waypoint_index][4],
self.poses[self.waypoint_index][5])
_distance_waypoint_plane = geo.dot_product_vectors(
_pos_vector, _waypoint_vector)
print("plane distance {}m".format(_distance_waypoint_plane))
#if d < _threshold or (d < 2 and delta_d < 0):
if (_distance_waypoint_plane < 0
and d < _distance_threshold) or self.waypoint_index == -1:
self._last_distance = None
self.waypoint_index += 1
if self.waypoint_index == len(self.poses):
self.waypoint_index = 0
# Advance waypoint target 1m in front of the gate
waypoint = Point(self.poses[self.waypoint_index][0],
self.poses[self.waypoint_index][1],
self.poses[self.waypoint_index][2])
waypoint_vector = geo.vector_from_RPY(
self.poses[self.waypoint_index][3],
self.poses[self.waypoint_index][4],
self.poses[self.waypoint_index][5])
advance_wp = Point(waypoint.x + waypoint_vector.x,
waypoint.y + waypoint_vector.y,
waypoint.z + waypoint_vector.z)
'''
# Method 1. Speed vector is from actual position to gate
waypoint = Point(
self.poses[self.waypoint_index][0],
self.poses[self.waypoint_index][1],
self.poses[self.waypoint_index][2]
)
speed_vector = unit_vector(msg.position, waypoint)
'''
'''
# Method 2. Speed vector is from previous waypoint to next waypoint
last_waypoint_index = self.waypoint_index - 1
if last_waypoint_index < 0: last_waypoint_index = 0
last_waypoint = Point(
self.poses[last_waypoint_index][0],
self.poses[last_waypoint_index][1],
self.poses[last_waypoint_index][2]
)
if self.waypoint_index > 0:
speed_vector = unit_vector_two_points(last_waypoint, waypoint)
else:
speed_vector = unit_vector_two_points(msg.position, waypoint)
'''
'''
# Method 3. Speed vector is the pose of the gate
speed_vector = waypoint_vector
'''
# Method 4. Speed vector is from current waypoint to next waypoint
next_waypoint_index = self.waypoint_index + 1
if next_waypoint_index >= len(self.poses): next_waypoint_index = 0
next_waypoint = Point(self.poses[next_waypoint_index][0],
self.poses[next_waypoint_index][1],
self.poses[next_waypoint_index][2])
speed_vector = geo.unit_vector_two_points(msg.position,
next_waypoint)
# Scale speed
speed_vector.x *= _exit_speed
speed_vector.y *= _exit_speed
speed_vector.z *= _exit_speed
self.command_pub.publish_pose_speed_command(
poses[self.waypoint_index][0], poses[self.waypoint_index][1],
poses[self.waypoint_index][2], poses[self.waypoint_index][3],
poses[self.waypoint_index][4], poses[self.waypoint_index][5],
speed_vector)
'''
def callbackImage(self, data):
#global first_time
ids = []
try:
src_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
(rows, cols, channels) = src_image.shape
except CvBridgeError as e:
print(e)
#-- 180 deg rotation matrix around x axis
R_flip = np.zeros((3, 3), dtype=np.float)
R_flip[0, 0] = +1.0
R_flip[1, 1] = -1.0
R_flip[2, 2] = -1.0
#-- Convert in gray scale\n",
gray = cv2.cvtColor(
src_image, cv2.COLOR_BGR2GRAY
) #-- remember, OpenCV stores color images in Blue, Green, Red
#-- Find all the aruco markers in the image\n",
corners, ids, rejected = aruco.detectMarkers(
image=gray,
dictionary=aruco_dict,
parameters=parameters,
cameraMatrix=camera_matrix,
distCoeff=camera_distortion)
#rospy.loginfo("ids[]: %f", len(ids[0]))
#int(ids) == id_to_find:
if ids[0] != None and ids[0] == id_to_find:
#if (int(ids[0]) != None and int(ids[0]) == id_to_find):
#-- ret= [rvec,tvec, ?]
#-- array of rotation and position of each marker in camera frame
#-- rvec = [[rvec_1, [rvec2], ...]] attitude of the marker respect to camera frame
#-- tvec = [[tvec_1, [tvec2], ...]] position of the marker in camera frame
ret = aruco.estimatePoseSingleMarkers(corners, marker_size,
camera_matrix,
camera_distortion)
#-- Unpack the output, get only the first\n",
rvec, tvec = ret[0][0, 0, :], ret[1][0, 0, :]
#-- Draw the detected marker and put a reference frame over it\n",
aruco.drawDetectedMarkers(src_image, corners)
aruco.drawAxis(src_image, camera_matrix, camera_distortion, rvec,
tvec, 0.3)
#-- Obtain the rotation matrix tag->camera
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_tc = R_ct.T # function transpose() with '.T'
#-- Get the attitude in terms of euler 321 (Needs to be flipped first)
pitch_marker, roll_marker, yaw_marker = rotationMatrixToEulerAngles(
R_tc)
#-- Now get Position and attitude f the camera respect to the marker
#pos_camera = -R_tc*np.matrix(tvec).T
pitch_camera, roll_camera, yaw_camera = rotationMatrixToEulerAngles(
R_flip * R_tc)
pos_camera = Point(-tvec[0], tvec[1], tvec[2])
#-- Print 'X' in the center of the camera
cv2.putText(src_image, "X", (cols / 2, rows / 2), font, 1,
(0, 0, 255), 2, cv2.LINE_AA)
###############################################################################
#-- Print the tag position in camera frame
str_position = "Position x = %4.0f y = %4.0f z = %4.0f" % (
pos_camera.x, pos_camera.y, pos_camera.z)
cv2.putText(src_image, str_position, (0, 30), font, 2,
(255, 255, 0), 2, cv2.LINE_AA)
#-- Get the attitude of the camera respect to the frame
str_attitude = "Attitude pitch = %4.0f roll = %4.0f yaw = %4.0f" % (
math.degrees(0), math.degrees(0), math.degrees(yaw_camera))
cv2.putText(src_image, str_attitude, (0, 60), font, 2,
(255, 255, 0), 2, cv2.LINE_AA)
###############################################################################
#cv2.imshow("Image-Aruco", src_image)
#cv2.waitKey(1)
aruco_odom = Odometry()
#aruco_odom.header.stamp = rospy.Time.now()-first_time
aruco_odom.header.stamp = rospy.Time.now()
aruco_odom.header.frame_id = "odom_aruco"
aruco_odom.header.seq = self.Keyframe_aruco
aruco_odom.child_frame_id = "drone_base"
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, yaw_camera)
# set the position
aruco_odom.pose.pose = Pose(pos_camera, Quaternion(*odom_quat))
tf_br = tf.TransformBroadcaster()
tf_br.sendTransform((pos_camera.x, pos_camera.y, pos_camera.z),
odom_quat, aruco_odom.header.stamp,
"drone_base", "odom_aruco") #world
euler_ori = Twist()
euler_ori.linear.x = -tvec[0]
euler_ori.linear.y = tvec[1]
euler_ori.linear.z = tvec[2]
euler_ori.angular.x = math.degrees(0)
euler_ori.angular.y = math.degrees(0)
euler_ori.angular.z = math.degrees(yaw_camera)
self.Keyframe_aruco += 1
euler_ori = Twist()
euler_ori.linear.x = -tvec[0]
euler_ori.linear.y = tvec[1]
euler_ori.linear.z = tvec[2]
euler_ori.angular.x = math.degrees(0)
euler_ori.angular.y = math.degrees(0)
euler_ori.angular.z = math.degrees(yaw_camera)
#rospy.loginfo('Id detected!')
try:
self.pose_aruco_pub.publish(aruco_odom)
self.orientation_euler_pub.publish(euler_ori)
except:
rospy.loginfo('No publish!')
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(src_image, "bgr8"))
except CvBridgeError as e:
print(e)
else:
rospy.loginfo('No Id detected!')
