def calculate_mk2_ik(self):
#goal_point = Point(0.298, -0.249, -0.890) home position
goal_pose = Pose()
# Goto position 1
goal_point = Point(0.298, -0.249, -0.890)
#goal_point = Point(0.298, -0.5, -1.0)
goal_pose.position = goal_point
quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
goal_pose.orientation = Quaternion(*quat.tolist())
moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False)
rospy.loginfo("Sending goal...")
self.moveit_ac.send_goal(moveit_goal)
rospy.loginfo("Waiting for result...")
self.moveit_ac.wait_for_result(rospy.Duration(10.0))
moveit_result = self.moveit_ac.get_result()
# Goto position 2
goal_point = Point(0.305, -0.249, -1.05)
goal_pose.position = goal_point
quat = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
goal_pose.orientation = Quaternion(*quat.tolist())
moveit_goal = self.create_move_group_pose_goal(goal_pose, group="manipulator", end_link_name="link_7", plan_only=False)
rospy.loginfo("Sending goal...")
self.moveit_ac.send_goal(moveit_goal)
rospy.loginfo("Waiting for result...")
self.moveit_ac.wait_for_result(rospy.Duration(10.0))
moveit_result = self.moveit_ac.get_result()
def listen_imu(self, imu):
# node can sleep for 16 out of 20 ms without dropping any messages
# rospy.sleep(16.0/1000.0)
# self.NN += 1
# if rospy.get_time() - self.t0 > 1:
# rospy.logfatal("Published {} messages in 1 s, {} Hz".format(self.NN, self.NN))
# self.NN, self.t0 = 0, rospy.get_time()
current_orientation = np.array([imu.orientation.x,
imu.orientation.y,
imu.orientation.z,
imu.orientation.w])
current_angular_speed = np.array([imu.angular_velocity.x,
imu.angular_velocity.y,
0.0])
current_rpy = list(transformations.euler_from_quaternion(current_orientation))
# No care about yaw, but must be close to zero
current_rpy[-1] = 0
desired_rpy = list(transformations.euler_from_quaternion(self.desired_orientation))
desired_rpy[-1] = 0
corrected = self.rp_pid(desired_rpy, current_rpy, current_angular_speed)
corrected_orientation = transformations.quaternion_multiply(
transformations.quaternion_from_euler(*corrected),
transformations.quaternion_from_euler(*current_rpy))
setpoint_pose = Pose(self.desired_position, corrected_orientation)
servo_angles = self.platform.ik(setpoint_pose)
rospy.logdebug(
"Servo angles (deg): {}".format(np.rad2deg(servo_angles)))
self.publish_servo_angles(servo_angles)
def in_odom_callback(self, in_odom_msg):
q = np.array([in_odom_msg.pose.pose.orientation.x,
in_odom_msg.pose.pose.orientation.y,
in_odom_msg.pose.pose.orientation.z,
in_odom_msg.pose.pose.orientation.w])
p = np.array([in_odom_msg.pose.pose.position.x,
in_odom_msg.pose.pose.position.y,
in_odom_msg.pose.pose.position.z])
e = tfs.euler_from_quaternion(q, 'rzyx')
wqb = tfs.quaternion_from_euler(e[0], e[1], e[2], 'rzyx')
wqc = tfs.quaternion_from_euler(e[0], 0.0, 0.0, 'rzyx')
#### odom ####
odom_msg = in_odom_msg
assert(in_odom_msg.header.frame_id == self.frame_id_in)
odom_msg.header.frame_id = self.frame_id_out
odom_msg.child_frame_id = ""
self.out_odom_pub.publish(odom_msg)
#### tf ####
if self.broadcast_tf and self.tf_pub_flag:
self.tf_pub_flag = False
if not self.frame_id_in == self.frame_id_out:
br.sendTransform((0.0, 0.0, 0.0),
tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'),
odom_msg.header.stamp,
self.frame_id_in,
self.frame_id_out)
if not self.world_frame_id == self.frame_id_out:
br.sendTransform((0.0, 0.0, 0.0),
tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'),
odom_msg.header.stamp,
self.world_frame_id,
self.frame_id_out)
br.sendTransform((p[0], p[1], p[2]),
wqb,
odom_msg.header.stamp,
self.body_frame_id,
self.world_frame_id)
br.sendTransform(((p[0], p[1], p[2])),
wqc,
odom_msg.header.stamp,
self.intermediate_frame_id,
self.world_frame_id)
#### path ####
pose = PoseStamped()
pose.header = odom_msg.header
pose.pose.position.x = p[0]
pose.pose.position.y = p[1]
pose.pose.position.z = p[2]
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.path.append(pose)
def publish_gaussians():
# Use TF to calculate relative area definitions
br.sendTransform((0.9718, 2.6195, 0),
quaternion_from_euler(0,0,3.1415) ,
rospy.Time.now(),
"drawer_gaussian_mean",
"map")
br.sendTransform((2.17495, 2.6431, 0),
quaternion_from_euler(0,0,0) ,
rospy.Time.now(),
"table_gaussian_mean",
"map")
br.sendTransform((0.76371, 2.17398, 0),
quaternion_from_euler(0,0,3.1415) ,
rospy.Time.now(),
"stove_gaussian_mean",
"map")
br.sendTransform((0.77843, 3.21516, 0),
quaternion_from_euler(0,0,3.1415) ,
rospy.Time.now(),
"cupboard_gaussian_mean",
"map")
def drawer(self, point):
#Prepare
GRIPPER_OPEN = .08
GRIPPER_CLOSE = .003
MAX_HANDLE_SIZE = .03
linear_movement = self.behaviors.movement
gripper = self.robot.left_gripper
#gripper.open(True, position=GRIPPER_OPEN)
#linear_movement.gripper_open()
self.open_gripper()
linear_movement.move_absolute((self.start_location_drawer[0],
#np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0))),
np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0))),
stop='pressure_accel', pressure=1000)
#Reach
success, reason, touchloc_bl = self.behaviors.reach(point, 300, #np.matrix([0.0, 0, 0]).T,
reach_direction=np.matrix([0.1, 0, 0]).T,
orientation=np.matrix(tr.quaternion_from_euler(np.radians(90.), 0, 0)))
#Error recovery
if not success:
error_msg = 'Reach failed due to "%s"' % reason
rospy.loginfo(error_msg)
rospy.loginfo('Failure recovery: moving back')
try:
linear_movement.move_relative_gripper(np.matrix([-.25,0,0]).T, stop='pressure_accel', pressure=300)
except lm.RobotSafetyError, e:
rospy.loginfo('robot safety error %s' % str(e))
self.behaviors.movement.move_absolute(self.start_location_drawer, stop='accel', pressure=300)
return False, 'reach failed', point
def publish_gaussians():
# Use TF to calculate relative area definitions
# Gaussians depending on edges of furniture and object/furniture positions
br.sendTransform((0.205067,-0.0136504 , 0),
quaternion_from_euler(0,0,0),
rospy.Time.now(),
"table_gauss_mean",
"plate_edge")
br.sendTransform((0.366749, 0.088665, 0),
quaternion_from_euler(0,0,0),
rospy.Time.now(),
"drawer55_gauss_mean",
"drawer55_edge")
br.sendTransform((0.1462250, 0.1426716 , 0),
quaternion_from_euler(0,0,0),
rospy.Time.now(),
"stove_gauss_mean",
"placemat_edge")
# TODO Here, i manually set this to minus, but this should actually be calculated
# based on the location of the door of the cupboard
br.sendTransform((0.42658, -0.107175 , 0),
quaternion_from_euler(0,0,0),
rospy.Time.now(),
"cupboard_gauss_mean",
"drawer115_edge")
def plan_path_cb(self, req):
rospy.loginfo('Received [plan_path] request.')
p1, p2 = (req.start.x, req.start.y), (req.end.x, req.end.y)
path = self.planner.plan(p1, p2)
try:
self.visualizer.visualize(self.planner.graph)
except:
# If a student code in RandomizedRoadmapPlanner does not have a
# graph object, or it is not compatible with GraphVisualizer,
# silently skip visualization
pass
if not len(path):
rospy.logwarn(self.__class__.__name__ + ': Failed to find a path.')
return None
# Convert the path output by the planner to ROS message
path_msg = Path()
path_msg.header.frame_id = 'odom'
path_msg.header.stamp = rospy.Time.now()
q_start = quaternion_from_euler(0, 0, req.start.theta)
q_end = quaternion_from_euler(0, 0, req.end.theta)
for pt in path:
p = PoseStamped()
p.pose.position = Point(pt[0], pt[1], 0.0)
p.pose.orientation = Quaternion(*q_start)
path_msg.poses.append(p)
# The randomized roadmap planner does not consider orientation, thus
# we manually add a final path segment which rotates the robot to the
# desired orientation
last = path_msg.poses[-1]
last.pose.orientation = Quaternion(*q_end)
path_msg.poses.append(last)
return path_msg
def test_avg_tag_localization(self):
self.setup()
# Publish a matching apriltag observation (from the static publisher in the test file)
msg_tag = AprilTags()
tag_100 = TagDetection()
tag_100.id = 100
trans = tag_100.transform.translation
rot = tag_100.transform.rotation
(trans.x, trans.y, trans.z) = (1,-1,0)
(rot.x, rot.y, rot.z, rot.w) = (0,0,1,0)
msg_tag.detections.append(tag_100)
tag_101 = TagDetection()
tag_101.id = 101
trans = tag_101.transform.translation
rot = tag_101.transform.rotation
(trans.x, trans.y, trans.z) = (1,0,0)
(rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_euler(0,0,np.pi/2)
msg_tag.detections.append(tag_101)
self.pub_tag.publish(msg_tag)
# Wait for the node to publish a robot transform
tfbuf = tf2_ros.Buffer()
tfl = tf2_ros.TransformListener(tfbuf)
try:
Tr_w = tfbuf.lookup_transform("world", "duckiebot", rospy.Time(), rospy.Duration(5))
except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
self.assertFalse(True, "Test timed out waiting for the transform to be broadcast.")
trans = (Tr_w.transform.translation.x, Tr_w.transform.translation.y, Tr_w.transform.translation.z)
rot = (Tr_w.transform.rotation.x, Tr_w.transform.rotation.y, Tr_w.transform.rotation.z, Tr_w.transform.rotation.w)
numpy.testing.assert_array_almost_equal(trans, (2, 0.5, 0))
numpy.testing.assert_array_almost_equal(rot, tr.quaternion_from_euler(0,0,3*np.pi/4))
def publish_gaussians():
# Gaussians depending on edges of furniture and object/furniture positions
br.sendTransform((0.205067,-0.0136504 , 0),
quaternion_from_euler(0,0,0),
rospy.Time.now(),
"table_gauss_mean",
"plate_edge")
br.sendTransform((0.366749, 0.088665, 0),
quaternion_from_euler(0,0,0),
rospy.Time.now(),
"drawer_gauss_mean",
"drawer_edge")
br.sendTransform((0.1462250, 0.1426716 , 0),
quaternion_from_euler(0,0,0),
rospy.Time.now(),
"stove_gauss_mean",
"placemat_edge")
br.sendTransform((0.42658, 0.107175 , 0),
quaternion_from_euler(0,0,0),
rospy.Time.now(),
"cupboard_gauss_mean",
"cupboard_edge")
def follow_pose_with_admitance_by_ft(self):
fx, fy, fz = self.get_force_movement()
rospy.loginfo(
"fx, fy, fz: " + str((round(fx, 3), round(fy, 3), round(fz, 3))))
ps = Pose()
if abs(fx) > self.fx_deadband:
ps.position.x = (fx * self.fx_scaling) * self.frame_fixer.fx
if abs(fy) > self.fy_deadband:
ps.position.y = (fy * self.fy_scaling) * self.frame_fixer.fy
if abs(fz) > self.fz_deadband:
ps.position.z = (fz * self.fz_scaling) * self.frame_fixer.fz
tx, ty, tz = self.get_torque_movement()
rospy.loginfo(
"tx, ty, tz: " + str((round(tx, 3), round(ty, 3), round(tz, 3))))
roll = pitch = yaw = 0.0
if abs(tx) > self.tx_deadband:
roll += (tx * self.tx_scaling) * self.frame_fixer.tx
if abs(ty) > self.ty_deadband:
pitch += (ty * self.ty_scaling) * self.frame_fixer.ty
if abs(tz) > self.tz_deadband:
yaw += (tz * self.tz_scaling) * self.frame_fixer.tz
q = quaternion_from_euler(roll, pitch, yaw)
ps.orientation = Quaternion(*q)
o = self.last_pose_to_follow.pose.orientation
r_lastp, p_lastp, y_lastp = euler_from_quaternion([o.x, o.y, o.z, o.w])
final_roll = r_lastp + roll
final_pitch = p_lastp + pitch
final_yaw = y_lastp + yaw
self.current_pose.pose.orientation = Quaternion(*quaternion_from_euler(final_roll,
final_pitch,
final_yaw))
self.current_pose.pose.position.x = self.last_pose_to_follow.pose.position.x + \
ps.position.x
self.current_pose.pose.position.y = self.last_pose_to_follow.pose.position.y + \
ps.position.y
self.current_pose.pose.position.z = self.last_pose_to_follow.pose.position.z + \
ps.position.z
self.current_pose.pose.position.x = self.sanitize(self.current_pose.pose.position.x,
self.min_x,
self.max_x)
self.current_pose.pose.position.y = self.sanitize(self.current_pose.pose.position.y,
self.min_y,
self.max_y)
self.current_pose.pose.position.z = self.sanitize(self.current_pose.pose.position.z,
self.min_z,
self.max_z)
self.current_pose.header.stamp = rospy.Time.now()
if self.send_goals: # send MODIFIED GOALS
self.pose_pub.publish(self.current_pose)
else:
self.last_pose_to_follow.header.stamp = rospy.Time.now()
self.pose_pub.publish(self.last_pose_to_follow)
self.debug_pose_pub.publish(self.current_pose)
def execute(self, userdata):
rospy.loginfo("Creating goal to put robot in front of handle")
pose_handle = Pose()
if userdata.door_detection_data_in_base_link.handle_side == "left" or userdata.door_detection_data_in_base_link.handle_side == "right": # closed door
(r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_handle.pose.orientation.x,
userdata.door_detection_data_in_base_link.door_handle.pose.orientation.y,
userdata.door_detection_data_in_base_link.door_handle.pose.orientation.z,
userdata.door_detection_data_in_base_link.door_handle.pose.orientation.w)) # gives back r, p, y
theta += 3.1416 # the orientation of the door is looking towards the robot, we need the inverse
pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta)) # orientation to look parallel to the door
pose_handle.position.x = userdata.door_detection_data_in_base_link.door_handle.pose.position.x - 0.4 # to align the shoulder with the handle
pose_handle.position.y = userdata.door_detection_data_in_base_link.door_handle.pose.position.y + 0.2 # refer to upper comment
pose_handle.position.z = 0.0 # we dont need the Z for moving
userdata.door_handle_pose_goal = pose_handle
else: # open door
# if it's open... just cross it?
(r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_position.pose.orientation.x,
userdata.door_detection_data_in_base_link.door_position.pose.orientation.y,
userdata.door_detection_data_in_base_link.door_position.pose.orientation.z,
userdata.door_detection_data_in_base_link.door_position.pose.orientation.w)) # gives back r, p, y
theta += 3.1416 # the orientation of the door is looking towards the robot, we need the inverse
pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta)) # orientation to look parallel to the door
pose_handle.position.x = userdata.door_detection_data_in_base_link.door_position.pose.position.x + 1.0 # enter the door
pose_handle.position.y = userdata.door_detection_data_in_base_link.door_position.pose.position.y
userdata.door_handle_pose_goal = pose_handle
rospy.loginfo("Move base goal: \n" + str(pose_handle))
return succeeded
def create_axes_markers(self, msg): x_marker = Marker() x_marker.type = Marker.CYLINDER x_marker.scale.x = msg.scale * 0.05 x_marker.scale.y = msg.scale * 0.05 x_marker.scale.z = msg.scale * 0.25 x_marker.color.a = 1.0 y_marker = copy.deepcopy(x_marker) z_marker = copy.deepcopy(x_marker) x_marker.pose.position.x += x_marker.scale.z * 0.5 y_marker.pose.position.y += y_marker.scale.z * 0.5 z_marker.pose.position.z += z_marker.scale.z * 0.5 x_marker.color.r = 1.0 y_marker.color.g = 1.0 z_marker.color.b = 1.0 x_quat = tr.quaternion_from_euler(0, np.deg2rad(90), 0) x_marker.pose.orientation.x = x_quat[0] x_marker.pose.orientation.y = x_quat[1] x_marker.pose.orientation.z = x_quat[2] x_marker.pose.orientation.w = x_quat[3] y_quat = tr.quaternion_from_euler(np.deg2rad(-90), 0, 0) y_marker.pose.orientation.x = y_quat[0] y_marker.pose.orientation.y = y_quat[1] y_marker.pose.orientation.z = y_quat[2] y_marker.pose.orientation.w = y_quat[3] y_marker.id = 1 z_marker.id = 2 return x_marker, y_marker, z_marker
def pre_place_pose(ud):
robot.torso.high()
goal_pose = PoseStamped()
goal_pose.header.stamp = rospy.Time.now()
goal_pose.header.frame_id = 'map'
goal_pose.pose.position.x = -4.55638664735
goal_pose.pose.position.y = 4.99959353359
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 0.395625992))
ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01, 0.1)).execute({})
robot.torso.wait_for_motion_done()
robot.speech.speak('I am now raising my arm')
arm.resolve()._send_joint_trajectory([[-1.1, 0.044, 0.80, 0.297, 0.934, -0.95, 0.4]],
timeout=rospy.Duration(0))
arm.resolve().wait_for_motion_done()
goal_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.9656259917))
ControlToPose(robot, goal_pose, ControlParameters(0.5, 1.0, 0.3, 0.3, 0.3, 0.01, 0.1)).execute({})
arm.resolve().wait_for_motion_done()
arm.resolve()._send_joint_trajectory([[-0.92, 0.044, 0.80, 0.497, 0.934, -0.95, 0.4]],
timeout=rospy.Duration(0))
arm.resolve().wait_for_motion_done()
rospy.sleep(1.0)
return 'done'
def __init__ (self):
rospy.init_node('move_base', anonymous=True)
rospy.on_shutdown(self.shutdown)
quaternions = list()
square_size = 1.0 # meters
# The first two target orientations are 90 degrees (horizontal pointing left)
q_turn_angle = quaternion_from_euler(0, 0, pi / 2, axes='sxyz')
q = Quaternion(*q_turn_angle)
# Append the first turn
rospy.loginfo(q)
quaternions.append(q)
# Append the second turn
quaternions.append(q)
# The second two target orientations are 270 degrees (horizontal point right)
q_turn_angle = quaternion_from_euler(0, 0, 3 * pi / 2, axes='sxyz')
q = Quaternion(*q_turn_angle)
# Append the third turn
quaternions.append(q)
# Append hte fourth turn
quaternions.append(q)
waypoints = list()
waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))
waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))
waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))
waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
# Initialize the visualization markers for RViz
self.init_markers()
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
for i in range(4):
self.marker_pub.publish(self.markers)
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = waypoints[i]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
self.move()
def generate_grasp_poses(self, object_pose):
# Compute all the points of the sphere with step X
# http://math.stackexchange.com/questions/264686/how-to-find-the-3d-coordinates-on-a-celestial-spheres-surface
radius = self._grasp_desired_distance
ori_x = 0.0
ori_y = 0.0
ori_z = 0.0
sphere_poses = []
rotated_q = quaternion_from_euler(0.0, 0.0, math.radians(180))
yaw_qtty = int((self._max_degrees_yaw - self._min_degrees_yaw) / self._step_degrees_yaw) # NOQA
pitch_qtty = int((self._max_degrees_pitch - self._min_degrees_pitch) / self._step_degrees_pitch) # NOQA
info_str = "Creating poses with parameters:\n" + \
"Radius: " + str(radius) + "\n" \
"Yaw from: " + str(self._min_degrees_yaw) + \
" to " + str(self._max_degrees_yaw) + " with step " + \
str(self._step_degrees_yaw) + " degrees.\n" + \
"Pitch from: " + str(self._min_degrees_pitch) + \
" to " + str(self._max_degrees_pitch) + \
" with step " + str(self._step_degrees_pitch) + " degrees.\n" + \
"Total: " + str(yaw_qtty) + " yaw * " + str(pitch_qtty) + \
" pitch = " + str(yaw_qtty * pitch_qtty) + " grap poses."
rospy.loginfo(info_str)
# altitude is yaw
for altitude in range(self._min_degrees_yaw, self._max_degrees_yaw, self._step_degrees_yaw): # NOQA
altitude = math.radians(altitude)
# azimuth is pitch
for azimuth in range(self._min_degrees_pitch, self._max_degrees_pitch, self._step_degrees_pitch): # NOQA
azimuth = math.radians(azimuth)
# This gets all the positions
x = ori_x + radius * math.cos(azimuth) * math.cos(altitude)
y = ori_y + radius * math.sin(altitude)
z = ori_z + radius * math.sin(azimuth) * math.cos(altitude)
# this gets all the vectors pointing outside of the center
# quaternion as x y z w
q = quaternion_from_vectors([radius, 0.0, 0.0], [x, y, z])
# Cannot compute so the vectors are parallel
if q is None:
# with this we add the missing arrow
q = rotated_q
# We invert the orientations to look inwards by multiplying
# with a quaternion 180deg rotation on yaw
q = quaternion_multiply(q, rotated_q)
# We actually want roll to be always 0.0 so we approach
# the object with the gripper always horizontal
# this rotation can be tuned with the dynamic params
# multiplying later on
roll, pitch, yaw = euler_from_quaternion(q)
q = quaternion_from_euler(math.radians(0.0), pitch, yaw)
x += object_pose.pose.position.x
y += object_pose.pose.position.y
z += object_pose.pose.position.z
current_pose = Pose(
Point(x, y, z), Quaternion(*q))
sphere_poses.append(current_pose)
return sphere_poses
def _publish_tf(self, stamp):
# check that we know which frames we need to publish from
if self.map is None or self.base_frame is None:
rospy.loginfo('Not publishing until map and odometry frames found')
return
# calculate the mean position
x = np.mean([p.x for p in self.particles])
y = np.mean([p.y for p in self.particles])
theta = np.mean([p.theta for p in self.particles]) #TODO - wraparound
# map to base_link
map2base_frame = PyKDL.Frame(
PyKDL.Rotation.Quaternion(*transformations.quaternion_from_euler(0, 0, theta)),
PyKDL.Vector(x,y,0)
)
odom2base_frame = tf2_kdl.transform_to_kdl(self.tf_buffer.lookup_transform(
target_frame=self.odom_frame,
source_frame=self.base_frame,
time=stamp,
timeout=rospy.Duration(4.0)
))
# derive frame according to REP105
map2odom = map2base_frame * odom2base_frame.Inverse()
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.stamp = stamp
t.header.frame_id = self.map.frame
t.child_frame_id = self.odom_frame
t.transform.translation = Vector3(*map2odom.p)
t.transform.rotation = Quaternion(*map2odom.M.GetQuaternion())
br.sendTransform(t)
# for Debugging
if False:
t.header.stamp = stamp
t.header.frame_id = self.map.frame
t.child_frame_id = self.base_frame+"_old"
t.transform.translation = Vector3(*map2base_frame.p)
t.transform.rotation = Quaternion(*map2base_frame.M.GetQuaternion())
br.sendTransform(t)
if self.publish_cloud:
msg = PoseArray(
header=Header(stamp=stamp, frame_id=self.map.frame),
poses=[
Pose(
position=Point(p.x, p.y, 0),
orientation=Quaternion(*transformations.quaternion_from_euler(0, 0, p.theta))
)
for p in self.particles
]
)
self.pose_array.publish(msg)
def process_touch_pose(ud):
#########################################################################
# tranformation logic for manipulation
# put touch pose in torso frame
frame_B_touch = util.pose_msg_to_mat(ud.touch_click_pose)
torso_B_frame = self.get_transform("torso_lift_link",
ud.touch_click_pose.header.frame_id)
if torso_B_frame is None:
return 'tf_failure'
torso_B_touch_bad = torso_B_frame * frame_B_touch
# rotate pixel23d the right way
t_pos, t_quat = util.pose_mat_to_pq(torso_B_touch_bad)
# rotate so x axis pointing out
quat_flip_rot = tf_trans.quaternion_from_euler(0.0, np.pi/2.0, 0.0)
quat_flipped = tf_trans.quaternion_multiply(t_quat, quat_flip_rot)
# rotate around x axis so the y axis is flat
mat_flipped = tf_trans.quaternion_matrix(quat_flipped)
rot_angle = np.arctan(-mat_flipped[2,1] / mat_flipped[2,2])
quat_ortho_rot = tf_trans.quaternion_from_euler(rot_angle + np.pi, 0.0, 0.0)
quat_flipped_ortho = tf_trans.quaternion_multiply(quat_flipped, quat_ortho_rot)
torso_B_touch = util.pose_pq_to_mat(t_pos, quat_flipped_ortho)
# offset the touch location by the approach tranform
appr_B_tool = self.get_transform(self.tool_approach_frame, self.tool_frame)
if appr_B_tool is None:
return 'tf_failure'
torso_B_touch_appr = torso_B_touch * appr_B_tool
# project the approach back into the wrist
appr_B_wrist = self.get_transform(self.tool_frame,
self.arm + "_wrist_roll_link")
if appr_B_wrist is None:
return 'tf_failure'
torso_B_wrist = torso_B_touch_appr * appr_B_wrist
#########################################################################
# create PoseStamped
appr_wrist_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_wrist)
appr_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_touch_appr)
touch_tool_ps = util.pose_mat_to_stamped_msg('torso_lift_link',
torso_B_touch)
# visualization
self.wrist_pub.publish(appr_wrist_ps)
self.appr_pub.publish(appr_tool_ps)
self.touch_pub.publish(touch_tool_ps)
# set return values
ud.appr_wrist_mat = torso_B_wrist
ud.appr_tool_ps = appr_tool_ps
ud.touch_tool_ps = touch_tool_ps
return 'succeeded'
def motionToTrajectory(self, action, frame_id):
""" Convert an arm movement action into a trajectory. """
ps = PoseStamped()
ps.header.frame_id = frame_id
ps.pose = action.goal
pose = self._listener.transformPose(self.root, ps)
rospy.loginfo("Parsing move to:\n" + str(pose))
# create IK request
request = GetPositionIKRequest()
request.timeout = rospy.Duration(self.timeout)
request.ik_request.pose_stamped.header.frame_id = self.root;
request.ik_request.ik_link_name = self.tip;
request.ik_request.pose_stamped.pose.position.x = pose.pose.position.x
request.ik_request.pose_stamped.pose.position.y = pose.pose.position.y
request.ik_request.pose_stamped.pose.position.z = pose.pose.position.z
e = euler_from_quaternion([pose.pose.orientation.x,pose.pose.orientation.y,pose.pose.orientation.z,pose.pose.orientation.w])
e = [i for i in e]
if self.dof < 6:
# 5DOF, so yaw angle = atan2(Y,X-shoulder offset)
e[2] = atan2(pose.pose.position.y, pose.pose.position.x-self.offset)
if self.dof < 5:
# 4 DOF, so yaw as above AND no roll
e[0] = 0
q = quaternion_from_euler(e[0], e[1], e[2])
request.ik_request.pose_stamped.pose.orientation.x = q[0]
request.ik_request.pose_stamped.pose.orientation.y = q[1]
request.ik_request.pose_stamped.pose.orientation.z = q[2]
request.ik_request.pose_stamped.pose.orientation.w = q[3]
request.ik_request.ik_seed_state.joint_state.name = self.arm_solver_info.kinematic_solver_info.joint_names
request.ik_request.ik_seed_state.joint_state.position = [self.servos[joint] for joint in request.ik_request.ik_seed_state.joint_state.name]
# get IK, wiggle if needed
tries = 0
pitch = e[1]
print "roll", e[0]
while tries < 80:
try:
response = self._get_ik_proxy(request)
if response.error_code.val == response.error_code.SUCCESS:
break
else:
tries += 1
# wiggle gripper
pitch = e[1] + ((-1)**tries)*((tries+1)/2)*0.025
# update quaternion
q = quaternion_from_euler(e[0], pitch, e[2])
request.ik_request.pose_stamped.pose.orientation.x = q[0]
request.ik_request.pose_stamped.pose.orientation.y = q[1]
request.ik_request.pose_stamped.pose.orientation.z = q[2]
request.ik_request.pose_stamped.pose.orientation.w = q[3]
except rospy.ServiceException, e:
print "Service did not process request: %s"%str(e)
def on_ptu_state_msg(self, msg):
pan1 = -msg.position[0]
pan_q = quaternion_from_euler(0, 0, -pan1)
tilt_q = quaternion_from_euler(0, -msg.position[1], 0)
b2 = tf.TransformBroadcaster()
b2.sendTransform( (0.0, 0, 0.065),
tilt_q,
rospy.Time.from_sec(time.time()),
'rover_amalia_turret_tilt_hinge',
'rover_amalia_turret_tilt_base')
def axis_cb(data):
global broadcaster, base_frame, base_name
pan = math.pi + data.pan * math.pi / 180.
tilt = -data.tilt * math.pi / 180.
q = quaternion_from_euler(0,0,pan)
now = rospy.Time.now()
broadcaster.sendTransform((0,0,0),
q,now,base_name+"/pan",base_frame)
q = quaternion_from_euler(0,tilt,0)
broadcaster.sendTransform((0,0,0),
q,now,base_name+"/tilt",base_name+"/pan")
def increment_reference(self):
'''
Steps the model reference (trajectory to track) by one self.timestep.
'''
# Frame management quantities
R_ref = trns.quaternion_matrix(self.q_ref)[:3, :3]
y_ref = trns.euler_from_quaternion(self.q_ref)[2]
# Convert body PD gains to world frame
kp = R_ref.dot(self.kp_body).dot(R_ref.T)
kd = R_ref.dot(self.kd_body).dot(R_ref.T)
# Compute error components (desired - reference), using "smartyaw"
p_err = self.p_des - self.p_ref
v_err = -self.v_ref
w_err = -self.w_ref
if npl.norm(p_err) <= self.heading_threshold:
q_err = trns.quaternion_multiply(self.q_des, trns.quaternion_inverse(self.q_ref))
else:
q_direct = trns.quaternion_from_euler(0, 0, np.angle(p_err[0] + (1j * p_err[1])))
q_err = trns.quaternion_multiply(q_direct, trns.quaternion_inverse(self.q_ref))
y_err = trns.euler_from_quaternion(q_err)[2]
# Combine error components into error vectors
err = np.concatenate((p_err, [y_err]))
errdot = np.concatenate((v_err, [w_err]))
wrench = (kp.dot(err)) + (kd.dot(errdot))
# Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts
B = np.concatenate((R_ref.dot(self.thruster_directions.T), R_ref.dot(self.lever_arms.T)))
B_3dof = np.concatenate((B[:2, :], [B[5, :]]))
command = self.thruster_mapper(wrench, B_3dof)
wrench_saturated = B.dot(command)
# Use model drag to find drag force on virtual boat
twist_body = R_ref.T.dot(np.concatenate((self.v_ref, [self.w_ref])))
D_body = np.zeros_like(twist_body)
for i, v in enumerate(twist_body):
if v >= 0:
D_body[i] = self.D_body_positive[i]
else:
D_body[i] = self.D_body_negative[i]
drag_ref = R_ref.dot(D_body * twist_body * abs(twist_body))
# Step forward the dynamics of the virtual boat
self.a_ref = (wrench_saturated[:2] - drag_ref[:2]) / self.mass_ref
self.aa_ref = (wrench_saturated[5] - drag_ref[2]) / self.inertia_ref
self.p_ref = self.p_ref + (self.v_ref * self.timestep)
self.q_ref = trns.quaternion_from_euler(0, 0, y_ref + (self.w_ref * self.timestep))
self.v_ref = self.v_ref + (self.a_ref * self.timestep)
self.w_ref = self.w_ref + (self.aa_ref * self.timestep)
def test_conversions():
push_trials = []
trial = push_learning.PushCtrlTrial()
trial.trial_start = push_learning.PushTrial()
trial.trial_end = push_learning.PushTrial()
cts0 = push_learning.ControlTimeStep()
cts0.x.x = 1.0
cts0.x.y = 2.0
cts0.x.theta = 0.5*pi
cts0.z = 2.0
cts0.ee.position.x = -0.5
cts0.ee.position.y = 3.0
cts0.ee.position.z = 2.5
q = quaternion_from_euler(0.0,0.0,0.5*pi)
cts0.ee.orientation.x = q[0]
cts0.ee.orientation.y = q[1]
cts0.ee.orientation.z = q[2]
cts0.ee.orientation.w = q[3]
cts0.u.linear.x = 2.0
cts0.u.linear.y = 1.0
cts0.u.angular.x = -pi
cts0.t = 0.0
cts1 = push_learning.ControlTimeStep()
cts1.x.x = cts0.x.x + 1.0
cts1.x.y = cts0.x.y + 1.5
cts1.x.theta = cts0.x.theta + 0.5*pi
cts1.z = cts0.z+0.25
cts1.ee.position.x = cts0.ee.position.x + 0.75
cts1.ee.position.y = cts0.ee.position.y + 1.75
cts1.ee.position.z = cts0.ee.position.z - 0.25
q = quaternion_from_euler(0.0,0.0,0.25*pi)
cts1.ee.orientation.x = q[0]
cts1.ee.orientation.y = q[1]
cts1.ee.orientation.z = q[2]
cts1.ee.orientation.w = q[3]
cts1.t = 0.25
trial.trial_trajectory.append(cts0)
trial.trial_trajectory.append(cts1)
trial.trial_trajectory.append(cts1)
(X, Y) = convert_push_trial_to_feat_vectors(trial)
for j, x in enumerate(X):
print j,':'
for i in xrange(len(x)):
print _FEAT_NAMES[i], '=', x[i]
print ''
for i in xrange(len(Y[0])):
print _TARGET_NAMES[i], '=', Y[0][i]
def turn_cb(userdata, nav_goal):
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.stamp = rospy.Time.now()
nav_goal.target_pose.header.frame_id = "/base_link"
nav_goal.target_pose.pose.position.x = 0.0
nav_goal.target_pose.pose.position.y = 0.0
nav_goal.target_pose.pose.position.z = 0.0
if userdata.times_turned == 2:
#Turns Left 45 degrees
nav_goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, ORIENTATION_PARAMETER))
elif userdata.times_turned == 3:
#Turns returns to pose and turns Right 45
nav_goal.target_pose.pose.orientation = Quaternion(*quaternion_from_euler(0, 0, -2*ORIENTATION_PARAMETER))
return nav_goal
def get_poses():
pose = Pose()
pose.position.x = x2_pos_r2_ee
pose.position.y = y2_pos_r2_ee
pose.position.z = z2_pos_r2_ee
pose.orientation = Quaternion(*quaternion_from_euler(phi2_r2_ee,teta2_r2_ee,sigma2_r2_ee))
poses = []
poses.append(copy.deepcopy(pose))
pose.position.x = x1_pos_r2_ee
pose.position.y = y1_pos_r2_ee
pose.position.z = z1_pos_r2_ee
pose.orientation = Quaternion(*quaternion_from_euler(phi1_r2_ee,teta1_r2_ee,sigma1_r2_ee))
poses.append(copy.deepcopy(pose))
return poses
def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz):
"""Create a SpawnModelRequest with the parameters of the cube given.
modelname: name of the model for gazebo
px py pz: position of the cube (and it's collision cube)
rr rp ry: rotation (roll, pitch, yaw) of the model
sx sy sz: size of the cube"""
cube = deepcopy(sdf_cube)
# Replace size of model
size_str = str(round(sx, 3)) + " " + \
str(round(sy, 3)) + " " + str(round(sz, 3))
cube = cube.replace('SIZEXYZ', size_str)
# Replace modelname
cube = cube.replace('MODELNAME', str(modelname))
req = SpawnModelRequest()
req.model_name = modelname
req.model_xml = cube
req.initial_pose.position.x = px
req.initial_pose.position.y = py
req.initial_pose.position.z = pz
q = quaternion_from_euler(rr, rp, ry)
req.initial_pose.orientation.x = q[0]
req.initial_pose.orientation.y = q[1]
req.initial_pose.orientation.z = q[2]
req.initial_pose.orientation.w = q[3]
return req
def __init__(self, normal=None):
# PointStamped TEMP!!!!!!!!!!!!!!
self.objectPoint = None
# gripper pose. Must both have frame_id of respective tool frame
self.leftGripperPose = None
self.rightGripperPose = None
# receptacle point. Must have frame_id of global (or main camera) frame
# is the exact place to drop off (i.e. don't need to do extra calcs to move away
self.receptaclePoint = None
# table normal. Must be according to global (or main camera) frame
if normal != None:
self.normal = normal
else:
# default to straight up
# self.normal = Util.makeQuaternion(.5**.5, 0, -.5**.5, 0)
# default to sideways
quat = tft.quaternion_from_euler(0, 0, -math.pi / 2)
self.normal = Util.makeQuaternion(quat[3], quat[0], quat[1], quat[2])
# Lock so two arms can access one ImageDetectionClass
# TEMP!!!!
self.objectLock = Lock()
# image processing to find object
self.objectProcessing = ImageProcessingClass()
# Temporary. Will eventually be placed with real image detection
# Will subscribe to camera feeds eventually
rospy.Subscriber("stereo_points_3d", PointStamped, self.stereoCallback)
rospy.Subscriber("/wide_stereo/right/image_rect", Image, self.imageCallback)
def create_placings_from_object_pose(self, posestamped):
""" Create a list of PlaceLocation of the object rotated every 15deg"""
place_locs = []
pre_grasp_posture = JointTrajectory()
# Actually ignored....
pre_grasp_posture.header.frame_id = self._grasp_pose_frame_id
pre_grasp_posture.joint_names = [
name for name in self._gripper_joint_names.split()]
jtpoint = JointTrajectoryPoint()
jtpoint.positions = [
float(pos) for pos in self._gripper_pre_grasp_positions.split()]
jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture)
pre_grasp_posture.points.append(jtpoint)
# Generate all the orientations every step_degrees_yaw deg
for yaw_angle in np.arange(0.0, 2.0 * pi, radians(self._step_degrees_yaw)):
pl = PlaceLocation()
pl.place_pose = posestamped
newquat = quaternion_from_euler(0.0, 0.0, yaw_angle)
pl.place_pose.pose.orientation = Quaternion(
newquat[0], newquat[1], newquat[2], newquat[3])
# TODO: the frame is ignored, this will always be the frame of the gripper
# so arm_tool_link
pl.pre_place_approach = self.createGripperTranslation(
Vector3(1.0, 0.0, 0.0))
pl.post_place_retreat = self.createGripperTranslation(
Vector3(-1.0, 0.0, 0.0))
pl.post_place_posture = pre_grasp_posture
place_locs.append(pl)
return place_locs
def process_markers(self, msg):
for marker in msg.markers:
# do some filtering basd on prior knowledge
# we know the approximate z coordinate and that all angles but yaw should be close to zero
euler_angles = euler_from_quaternion((marker.pose.pose.orientation.x,
marker.pose.pose.orientation.y,
marker.pose.pose.orientation.z,
marker.pose.pose.orientation.w))
angle_diffs = TransformHelpers.angle_diff(euler_angles[0],pi), TransformHelpers.angle_diff(euler_angles[1],0)
print angle_diffs, marker.pose.pose.position.z
if (marker.id in self.marker_locators and
3.0 <= marker.pose.pose.position.z <= 3.6 and
fabs(angle_diffs[0]) <= .4 and
fabs(angle_diffs[1]) <= .4):
print "FOUND IT!"
locator = self.marker_locators[marker.id]
xy_yaw = list(locator.get_camera_position(marker))
if self.is_flipped:
print "WE ARE FLIPPED!!!"
xy_yaw[2] += pi
print self.pose_correction
print self.phase_offset
xy_yaw[0] += self.pose_correction*cos(xy_yaw[2]+self.phase_offset)
xy_yaw[1] += self.pose_correction*sin(xy_yaw[2]+self.phase_offset)
orientation_tuple = quaternion_from_euler(0,0,xy_yaw[2])
pose = Pose(position=Point(x=-xy_yaw[0],y=-xy_yaw[1],z=0),
orientation=Quaternion(x=orientation_tuple[0], y=orientation_tuple[1], z=orientation_tuple[2], w=orientation_tuple[3]))
# TODO: use markers timestamp instead of now() (unfortunately, not populated currently by ar_pose)
pose_stamped = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="STAR"),pose=pose)
# TODO: use frame timestamp instead of now()
self.star_pose_pub.publish(pose_stamped)
self.fix_STAR_to_odom_transform(pose_stamped)
def plot(self, string):
walls = decode_string_to_tuplearray(string)
""" publish walls with cubes """
for i, (p1, p2) in enumerate(walls):
marker = get_marker('wall_%d' % i, Marker.CUBE)
# set scale
marker.scale.x = norm(p1-p2)
marker.scale.y = 20
marker.scale.z = 1e3
# set color
marker.color.r = 1.0
marker.color.a = 1.0
# set position
p = marker.pose.position
p.x, p.y = .5 * (p1 + p2)
p.z = .5e3
# set orientation
dp = p2 - p1
psi = acos(dp[0]/norm(dp))
o = marker.pose.orientation
o.x, o.y, o.z, o.w = quaternion_from_euler(0, 0, psi)
# publish
publisher.publish(marker)
for i in range(len(walls), self.prev_num_plotted_walls):
marker = get_marker('wall_%d' % i, Marker.CUBE)
p = marker.pose.position
p.x = -1e20
publisher.publish(marker)
self.prev_num_plotted_walls = len(walls)
def navigate(self):
rate = rospy.Rate(10) # 10hz
magnitude = 1 # in meters/sec
msg = SP.TwistStamped(
header=SP.Header(
frame_id="base_footprint", # no matter, plugin don't use TF
stamp=rospy.Time.now()), # stamp should update
)
i =0
#angle_cont
while not rospy.is_shutdown():
#print "publishing velocity"
self.throttle_update = self.pid_throttle.update(self.current_alt)
pid_offset = self.pid_alt.update(self.current_alt)
if self.use_pid:
msg.twist.linear = geometry_msgs.msg.Vector3(self.vx*magnitude, self.vy*magnitude, self.vz*magnitude+pid_offset)
else:
msg.twist.linear = geometry_msgs.msg.Vector3(self.vx*magnitude, self.vy*magnitude, self.vz*magnitude)
#msg.twist.linear = geometry_msgs.msg.Vector3(0, 1, self.vz*magnitude)
# Yaw won't work
yaw_degrees = self.yaw # North
yaw = radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
#msg.twist.angular = geometry_msgs.msg.Vector3(0,0,self.yaw_target)
if self.velocity_publish:
self.pub_vel.publish(msg)
rate.sleep()
i +=1
def urdfToMarkerArray(xml_robot,
frame_id_prefix='',
namespace=None,
rgba=None,
verbose=False):
"""
:param _robot_description:
:param frame_id_prefix:
:param namespace: if None, each link will its name. Otherwise, all links will have this namespace value value.
:param rgba:
:return: markers, a visualization_msgs/MarkerArray
"""
# rospy.init_node('urdf_to_rviz_converter', anonymous=True) # Initialize ROS node
# rospy.loginfo('Reading xml xacro file ...')
# xml_robot = URDF.from_parameter_server(_robot_description) # Parse robot description from param /robot_description
# Create colormaps to be used for coloring the elements. Each collection contains a color, each sensor likewise.
# graphics['collections']['colormap'] = cm.tab20b(np.linspace(0, 1, len(collections.keys())))
# for idx, collection_key in enumerate(sorted(collections.keys())):
# graphics['collections'][str(collection_key)] = {'color': graphics['collections']['colormap'][idx, :]}
markers = MarkerArray()
counter = 0
for link in xml_robot.links:
if verbose:
print("Analysing link: " + str(link.name))
print(link.name + ' has ' + str(len(link.visuals)) + ' visuals.')
for visual in link.visuals: # iterate through all visuals in the list
if not visual.geometry:
raise ValueError(
"Link name " + link.name +
"contains visual without geometry. Are you sure your "
"urdf/xacro is correct?")
else:
geom = visual.geometry
if verbose:
print("visual: " + str(visual))
x = y = z = 0 # origin xyz default values
qx = qy = qz = 0 # default rotation values
qw = 1
if visual.origin: # check if there is an origin
x, y, z = visual.origin.xyz[0], visual.origin.xyz[
1], visual.origin.xyz[2]
qx, qy, qz, qw = transformations.quaternion_from_euler(
visual.origin.rpy[0],
visual.origin.rpy[1],
visual.origin.rpy[2],
axes='sxyz')
if not rgba is None: # select link color
r, g, b, a = rgba[0], rgba[1], rgba[2], rgba[3]
elif visual.material:
r, g, b, a = visual.material.color.rgba
else:
r, g, b, a = 1, 1, 1, 1 # white by default
# define the frame_id setting the prefix if needed
frame_id = frame_id_prefix + link.name
if verbose:
print('frame id is ' + str(frame_id))
# define the namespace
if namespace is None:
namespace = link.name
else:
namespace = namespace
# Handle several geometries
if isinstance(geom, urdf_parser_py.urdf.Mesh):
if verbose:
print("Visual.geom of type urdf_parser_py.urdf.Mesh")
m = Marker(header=Header(frame_id=frame_id,
stamp=rospy.Time.now()),
ns=namespace,
id=counter,
frame_locked=True,
type=Marker.MESH_RESOURCE,
action=Marker.ADD,
lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=x, y=y, z=z),
orientation=Quaternion(x=qx,
y=qy,
z=qz,
w=qw)),
scale=Vector3(x=1.0, y=1.0, z=1.0),
color=ColorRGBA(r=r, g=g, b=b, a=a))
m.mesh_resource = geom.filename
m.mesh_use_embedded_materials = True
markers.markers.append(m)
counter += 1
elif isinstance(geom, urdf_parser_py.urdf.Box):
if verbose:
print("Visual.geom of type urdf_parser_py.urdf.Box")
sx = geom.size[0]
sy = geom.size[1]
sz = geom.size[2]
m = Marker(header=Header(frame_id=frame_id,
stamp=rospy.Time.now()),
ns=namespace,
id=counter,
frame_locked=True,
type=Marker.CUBE,
action=Marker.ADD,
lifetime=rospy.Duration(0),
pose=Pose(position=Point(x=x, y=y, z=z),
orientation=Quaternion(x=qx,
y=qy,
z=qz,
w=qw)),
scale=Vector3(x=sx, y=sy, z=sz),
color=ColorRGBA(r=r, g=g, b=b, a=a))
markers.markers.append(m)
counter += 1
else:
print("visuals:\n " + str(visual))
raise ValueError('Unknown visual.geom type' +
str(type(visual.geometry)) + " for link " +
link.name)
return markers
def _RequestTargetPose(self, desired_object):
# Perform a service request from FP
try:
# Handle preemption?
# if received a "End of mission" sort of message from FP
#start_pose,other_foot_pose = self._GetStartingFootPose()
if None == self._target_pose:
print("Request Target Pose to ", desired_object)
self._target_pose = self._foot_placement_client(
desired_object) # "target: 'Firehose'") #
print("Got from C23/C66 service the following Pose:",
self._target_pose)
delta_yaw, delta_trans = self._GetDeltaToObject(
desired_object, self._target_pose)
elif "Rotate_Translate_delta" == self._target_pose and not self._started_to_walk:
delta_yaw = self._delta_yaw
delta_trans = self._delta_trans
else:
delta_yaw = 0.0
delta_trans = Point()
delta_trans.x = 0.0
delta_trans.y = 0.0
delta_trans.z = 0.0
start_position = copy.copy(self._BDI_Static_pose.position)
start_orientation = euler_from_quaternion([
self._BDI_Static_pose.orientation.x,
self._BDI_Static_pose.orientation.y,
self._BDI_Static_pose.orientation.z,
self._BDI_Static_pose.orientation.w
])
self._foot_placement_path = []
if math.fabs(delta_yaw) > self._err_rot:
self._foot_placement_path = self._foot_placement_path + self._GetRotationDeltaFP_Path(
delta_yaw, start_position, start_orientation)
self._started_to_walk = True
if self._DistanceXY(delta_trans) > self._err_trans:
self._foot_placement_path = self._foot_placement_path + self._GetTranslationDeltaFP_Path(
delta_yaw, delta_trans, start_position, start_orientation)
self._started_to_walk = True
#listSteps = []
# if (math.fabs(delta_yaw) <= self._err_rot) and (self._DistanceXY(delta_trans) <= self._err_trans): # finished task
if [] == self._foot_placement_path and self._started_to_walk: # 1 == resp.done:
self._WalkingModeStateMachine.PerformTransition("Finished")
# if big error need to finish with error (didn't reach goal)
else:
if not self._started_to_walk:
self._started_to_walk = True
## Step in place: two first steps
self._foot_placement_path = self._GetTwoFirstStepFP_Path(
start_position, start_orientation, False)
listSteps = []
for desired in self._foot_placement_path:
command = AtlasSimInterfaceCommand()
step = command.step_params.desired_step
step.foot_index = desired.foot_index
step.swing_height = desired.clearance_height
step.pose.position.x = desired.pose.position.x
step.pose.position.y = desired.pose.position.y
step.pose.position.z = desired.pose.position.z
Q = quaternion_from_euler(desired.pose.ang_euler.x,
desired.pose.ang_euler.y,
desired.pose.ang_euler.z)
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
#print ("step command:",step)
listSteps.append(step)
self._LPP.SetPath(listSteps)
#print(listSteps)
except rospy.ServiceException, e:
print "Foot Placement Service call failed: %s" % e
print "============ Reference frame: %s" % group.get_planning_frame()
print "============ End effector: %s" % group.get_end_effector_link()
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
print "============"
print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
q = quaternion_from_euler(
-0.011, 1.57,
0.037) ## conversion from roll, pitch, and yaw into quaternions
pose_target.orientation.x = q[0]
pose_target.orientation.y = q[1]
pose_target.orientation.z = q[2]
pose_target.orientation.w = q[3]
pose_target.position.x = 0.7
pose_target.position.y = -0.3
pose_target.position.z = 0.8
group.set_pose_target(pose_target)
#group.setStartStateToCurrentState()
plan1 = group.plan()
'''
### start pose
start_pose = geometry_msgs.msg.Pose()
q = quaternion_from_euler(-0.011, 1.57, 0.037) ## conversion from roll, pitch, and yaw into quaternions
def _generate_places(self, target):
"""
Generate places (place locations), based on
https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/
baxter_pick_place/src/block_pick_place.cpp
为该位置的姿势创建姿势数组数据
"""
# Generate places:
places = []
now = rospy.Time.now()
print("---------------test3--------------------------")
for angle in numpy.arange(0.0, numpy.deg2rad(360.0),
numpy.deg2rad(1.0)):
# Create place location:
place = PlaceLocation()
place.place_pose.header.stamp = now
place.place_pose.header.frame_id = self._robot.get_planning_frame()
# Set target position:
place.place_pose.pose = copy.deepcopy(target)
# Generate orientation (wrt Z axis):
q = quaternion_from_euler(0.0, 0.0, angle)
place.place_pose.pose.orientation = Quaternion(*q)
# Generate pre place approach:生成前置位置方法：
place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist
place.pre_place_approach.min_distance = self._approach_retreat_min_dist
place.pre_place_approach.direction.header.stamp = now
place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame(
)
place.pre_place_approach.direction.vector.x = 0
place.pre_place_approach.direction.vector.y = 0
place.pre_place_approach.direction.vector.z = 0.1
#print("place1---test=====")
# Generate post place approach:
place.post_place_retreat.direction.header.stamp = now
place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame(
)
place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist
place.post_place_retreat.min_distance = self._approach_retreat_min_dist
place.post_place_retreat.direction.vector.x = 0
place.post_place_retreat.direction.vector.y = 0
place.post_place_retreat.direction.vector.z = 0.06
#print("place2---test=====")
# Add place:
places.append(place)
# Publish places (for debugging/visualization purposes):
self._publish_places(places)
"""
print(places)
print("------test4---------------------------------------")
"""
return places
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
loadUi(os.path.join(path, 'resources', 'path.ui'), self)
try:
rospy.wait_for_service('robot_send_command', timeout=5)
self.send_command = rospy.ServiceProxy(
'robot_send_command', SrvRobotCommand)
except:
rospy.loginfo('ERROR connecting to service robot_send_command.')
#self.pub = rospy.Publisher(
# import tf'robot_command_json', MsgRobotCommand, queue_size=10)
self.pub_marker_array = rospy.Publisher(
'visualization_marker_array', MarkerArray, queue_size=10)
self.btnLoadPath.clicked.connect(self.btnLoadPathClicked)
icon = QtGui.QIcon.fromTheme('document-open')
self.btnLoadPath.setIcon(icon)
self.btnSavePath.clicked.connect(self.btnSavePathClicked)
icon = QtGui.QIcon.fromTheme('document-save')
self.btnSavePath.setIcon(icon)
self.btnRunPath.clicked.connect(self.btnRunPathClicked)
icon = QtGui.QIcon.fromTheme('media-playback-start')
self.btnRunPath.setIcon(icon)
self.btnRunTest.clicked.connect(self.btnRunTestClicked)
self.btnRunTest.setIcon(icon)
self.btnDelete.clicked.connect(self.btnDeleteClicked)
self.btnLoadPose.clicked.connect(self.btnLoadPoseClicked)
self.btnStep.clicked.connect(self.btnStepClicked)
self.btnCancel.clicked.connect(self.btnCancelClicked)
self.listWidgetPoses.itemSelectionChanged.connect(self.lstPosesClicked)
self.listWidgetPoses.itemDoubleClicked.connect(self.qlistDoubleClicked)
self.testing = False
self.ok_command = "OK"
self.path_markers = PathMarkers()
# Parse robot description file
robot = URDF.from_parameter_server()
tcp = robot.joint_map['tcp0']
workobject = robot.joint_map['workobject']
tool = [tcp.origin.position,
list(tf.quaternion_from_euler(*tcp.origin.rotation))]
print 'Tool:', tool
workobject = [workobject.origin.position,
list(tf.quaternion_from_euler(*workobject.origin.rotation))]
print 'Workobject:', workobject
if rospy.has_param('/powder'):
powder = rospy.get_param('/powder')
else:
print '/powder param missing, loaded default'
powder = {'carrier': 5.0, 'shield': 10.0, 'stirrer': 20.0, 'turntable': 4.0}
print 'Powder:', powder
if rospy.has_param('/process'):
process = rospy.get_param('/process')
else:
print '/process param missing, loaded default'
process = {'focus': 0, 'power': 1000, 'speed': 8}
print 'Process:', process
self.jason = Jason()
self.jason.set_tool(tool)
self.jason.set_workobject(workobject)
self.jason.set_powder(
powder['carrier'], powder['stirrer'], powder['turntable'])
self.jason.set_process(process['speed'], process['power'])
self.tmrRunPath = QtCore.QTimer(self)
self.tmrRunPath.timeout.connect(self.timeRunPathEvent)
def main():
rospy.init_node('obstacle_avoidance', anonymous=True)
rospy.logwarn("GPS setpoints node is started")
K = Controller()
########## Subscribers ##########
# Subscriber: GPS
rospy.Subscriber("mavros/global_position/raw/fix", NavSatFix, K.gpsCb)
# Subscriber: Local pose
rospy.Subscriber("mavros/local_position/pose", PoseStamped, K.localPoseCb)
rospy.Subscriber("mavros/local_position/pose", PoseStamped,
K.transformationoffcu)
# Get landing state
rospy.Subscriber("mavros/extended_state", ExtendedState, K.landStateCb)
# FOR RPLIDAR
rospy.Subscriber("laser/scan", LaserScan, K.rangessCb)
########## Publishers ##########
# Publisher: PositionTarget
# avoid_pub = rospy.Publisher('mavros/setpoint_raw/local', PositionTarget, queue_size=1)
rate = rospy.Rate(10.0)
# Do initial checks
while ((K.current_lat * K.current_lon * K.current_alt)
== 0) and not rospy.is_shutdown():
rospy.loginfo(
'Waiting for current gps location to execute setWaypoints_and_Fence'
) # Initializes waypoints and fence in local x,y,z and checks to see if they make sense
rospy.sleep(0.1)
K.setWayoints_and_Fence()
K.resetStates()
# We need to send few setpoint messages, then activate OFFBOARD mode, to take effect
k = 0
while k < 10:
K.avoid_pub.publish(K.positionSp)
rate.sleep()
k = k + 1
K.positionSp.pose.position.x = K.current_local_x
K.positionSp.pose.position.y = K.current_local_y
K.positionSp.pose.position.z = K.takeoff_height
K.modes.setOffboardMode()
K.modes.setArm()
###################### TAKEOFF STUFF ####################
K.TAKEOFF = 1
#########################################################
while not rospy.is_shutdown():
if K.TAKEOFF:
rospy.logwarn('Vehicle is taking off')
#K.positionSp.header.frame_id = 'local_origin' # IS THIS NEEDED?
#K.positionSp.position.x = K.current_local_x
#K.positionSp.position.y = K.current_local_y
#K.positionSp.position.z = K.takeoff_height
#rospy.loginfo('Home X Position')
#rospy.loginfo(K.positionSp.position.x)
#rospy.loginfo('Home Y Position')
#rospy.loginfo(K.positionSp.position.y)
rospy.loginfo('Takeoff Height')
rospy.loginfo(K.positionSp.pose.position.z)
#avoid_pub.publish(K.positionSp)
# IS THIS NEEDED?
if abs(K.current_local_z - K.takeoff_height) < .1:
rospy.logwarn("Reached Takeoff Height")
K.resetStates()
K.WAYPOINT1 = 1
if K.WAYPOINT1:
angle = math.atan2((K.waypoint1_y - K.current_local_y),
(K.waypoint1_x - K.current_local_x))
K.check_and_avoid(angle)
if (K.minfront > 10):
rospy.loginfo('Vehicle heading to waypoint 1')
K.positionSp.header.frame_id = 'local_origin'
K.positionSp.pose.position.x = K.waypoint1_x
K.positionSp.pose.position.y = K.waypoint1_y
K.positionSp.pose.position.z = K.waypoint1_z
# North
yaw = angle
quaternion = quaternion_from_euler(0, 0, yaw)
K.positionSp.pose.orientation = Quaternion(*quaternion)
tempz1 = K.waypoint1_z
rospy.loginfo('Waypoint1_z')
rospy.loginfo(tempz1)
tempx = K.current_local_x - K.waypoint1_x
rospy.loginfo('Current_x - Waypoint1_x')
rospy.loginfo(tempx)
tempy = K.current_local_y - K.waypoint1_y
rospy.loginfo('Current_y - Waypoint1_y')
rospy.loginfo(tempy)
tempz = K.current_local_z - K.waypoint1_z
rospy.loginfo('Current_z - Waypoint1_z')
rospy.loginfo(tempz)
K.avoidancepar = 1
if abs(K.current_local_x - K.waypoint1_x) <= 1 and abs(
K.current_local_y - K.waypoint1_y) <= 1 and abs(
K.current_local_z - K.waypoint1_z
) <= 0.5: # Rules give a 3m radius from goal
rospy.loginfo(
"Current position close enough to desired waypoint")
rospy.loginfo("Reached waypoint 1")
K.resetStates()
K.WAYPOINT2 = 1
if K.WAYPOINT2:
angle = math.atan2((K.waypoint2_y - K.current_local_y),
(K.waypoint2_x - K.current_local_x))
K.check_and_avoid(angle)
if (K.minfront > 10):
rospy.loginfo('Vehicle heading to waypoint 2')
K.positionSp.header.frame_id = 'local_origin'
K.positionSp.pose.position.x = K.waypoint2_x
K.positionSp.pose.position.y = K.waypoint2_y
K.positionSp.pose.position.z = K.waypoint2_z
yaw = angle
quaternion = quaternion_from_euler(0, 0, yaw)
K.positionSp.pose.orientation = Quaternion(*quaternion)
if abs(K.current_local_x - K.waypoint2_x) <= 1 and abs(
K.current_local_y - K.waypoint2_y) <= 1 and abs(
K.current_local_z - K.waypoint2_z
) <= 0.5: # Rules give a 3m radius from goal
rospy.loginfo(
"Current position close enough to desired waypoint")
rospy.loginfo("Reached waypoint 2")
K.resetStates()
K.WAYPOINT3 = 1
K.avoidancepar = 1
if K.WAYPOINT3:
angle = math.atan2((K.waypoint2_y - K.current_local_y),
(K.waypoint2_x - K.current_local_x))
K.check_and_avoid(angle)
if (K.minfront > 10):
rospy.loginfo('Vehicle heading to waypoint 3')
K.positionSp.header.frame_id = 'local_origin'
K.positionSp.pose.position.x = K.waypoint3_x
K.positionSp.pose.position.y = K.waypoint3_y
K.positionSp.pose.position.z = K.waypoint3_z
yaw = angle
quaternion = quaternion_from_euler(0, 0, yaw)
K.positionSp.pose.orientation = Quaternion(*quaternion)
if abs(K.current_local_x - K.waypoint3_x) <= 1 and abs(
K.current_local_y - K.waypoint3_y) <= 1 and abs(
K.current_local_z - K.waypoint3_z
) <= 0.5: # Rules give a 3m radius from goal
rospy.loginfo(
'Current position close enough to desired waypoint')
rospy.loginfo('Reached waypoint 3')
K.resetStates()
K.GOHOME = 1
K.avoidancepar = 1
if K.GOHOME:
angle = math.atan2((K.home_y - K.current_local_y),
(K.home_x - K.current_local_x))
K.check_and_avoid(angle)
if (K.minfront > 10):
rospy.loginfo(
'Mission Complete - Vehicle heading back to home')
K.positionSp.header.frame_id = 'local_origin'
K.positionSp.pose.position.x = K.home_x
K.positionSp.pose.position.y = K.home_y
# K.positionSp.pose.position.z = K.home_z + 2 # I GUESS 2 IS A GOOD ALTITUDE TO RETURN
yaw = angle
quaternion = quaternion_from_euler(0, 0, yaw)
K.positionSp.pose.orientation = Quaternion(*quaternion)
if abs(K.current_local_x - K.home_x) <= 1 and abs(
K.current_local_y -
K.home_y) <= 1: # Rules give a 3m radius from goal
rospy.loginfo('Reached home')
K.resetStates()
K.LAND = 1
K.avoidancepar = 1
if K.LAND:
rospy.loginfo('Vehicle is landing')
K.modes.setAutoLandMode()
if K.IS_LANDED:
K.modes.setDisarm()
K.resetStates()
if K.HOVER:
rospy.logwarn('Vehicle in Hover mode until something else happens')
# NEED TO FIGURE OUT HOW TO EXIT THIS STATE
K.avoid_pub.publish(K.positionSp)
rate.sleep()
def __init__(self):
# Current GPS Coordinates
self.current_lat = 0.0
self.current_lon = 0.0
self.current_alt = 0.0
#Waypoints GPS Coordiantes
# self.home_lat = ENTER_HOME_COORDINATES
# self.home_lon = ENTER_HOME_COORDINATES # THIS WILL BE NEEDED EVENTUALLY BECAUSE THERE WILL BE TWO TAKEOFF/LANDING LOCATIONS
self.waypoint1_lat = 22.3118631 #22.3070405#22.317490
self.waypoint1_lon = 39.0952322 #39.1047228#39.097909 # Location of Testing Field at KAUST - a little ways down field
self.waypoint1_alt = 2.5
self.waypoint2_lat = 22.3118631 #22.3070405#22.317490
self.waypoint2_lon = 39.0952322 #39.1047228#39.097909 # Location of Testing Field at KAUST - a little ways down field
self.waypoint2_alt = 2.5
self.waypoint3_lat = 22.3118631 #22.3070405#22.317490
self.waypoint3_lon = 39.0952322 #39.1047228#39.097909 # Location of Testing Field at KAUST - a little ways down field
self.waypoint3_alt = 2.5
# Current local ENU coordinates
self.current_local_x = 0.0
self.current_local_y = 0.0
self.current_local_z = 0.0
#Waypoints ENU Coordinates
self.home_x = 0
self.home_y = 0
self.home_z = 0
self.avoidancepar = 1
self.waypoint1_x = 0
self.waypoint1_y = 30
self.waypoint1_z = 40
self.waypoint2_x = 0
self.waypoint2_y = 0
self.waypoint2_z = 40
self.waypoint3_x = 0
self.waypoint3_y = 0
self.waypoint3_z = 40
self.tf_listener = tf.TransformListener()
self.takeoff_height = 40
# Instantiate setpoint topic structures
self.positionSp = PoseStamped()
yaw_degrees = 0 # North
yaw = math.radians(yaw_degrees)
quaternion = quaternion_from_euler(0, 0, yaw)
self.positionSp.pose.orientation = Quaternion(*quaternion)
#self.positionSp.type_mask = int('010111111000', 2)
#self.positionSp.type_mask = int('010000111000', 2)
#self.positionSp.coordinate_frame=8
#defining the modes
self.modes = fcuModes()
# States
self.TAKEOFF = 0
self.WAYPOINT1 = 0
self.WAYPOINT2 = 0
self.WAYPOINT3 = 0
self.GOHOME = 0
self.LAND = 0
self.HOVER = 0
# AVOIDANCE STUFF
self.front = np.zeros(60)
self.left = np.zeros(30)
self.right = np.zeros(30)
self.back = np.zeros(60)
self.minfront = min(abs(self.front))
self.minleft = min(abs(self.left))
self.minright = min(abs(self.right))
self.minback = min(abs(self.back))
self.positionSp2 = PositionTarget()
self.positionSp2.type_mask = int('010111111000', 2)
#self.positionSp.type_mask = int('010000111000', 2)
self.positionSp2.coordinate_frame = 8
self.avoid_pub = rospy.Publisher('mavros/setpoint_position/local',
PoseStamped,
queue_size=1)
self.avoid_pub2 = rospy.Publisher('mavros/setpoint_raw/local',
PositionTarget,
queue_size=1)
def initialize_particle_cloud(self):
# Figure out height and width of map
width = self.map.info.width
height = self.map.info.height
# Map is a 384 x 384 grid
# Create a new array that stores the locations of all points on map that are empty
full_array = []
for i in range(width):
for j in range(height):
indx = (i * width + j)
if self.map.data[indx] == 0:
full_array.append(indx)
# Set all our initial particles to empty
initial_particle_set = []
# Create a random sample of size n from all the empty locations on map
new_sample = rand.sample(full_array, self.num_particles)
# Tranform each point into real coordinates and add a randomized orientation
for part in new_sample:
# Find the origin and resolution of the map
# Recalculate the x and y values of the randomly sampled points to fit our map
resolution = self.map.info.resolution
x_origin = self.map.info.origin.position.x
y_origin = self.map.info.origin.position.y
part = convert_to_real_coords(part, width, x_origin, y_origin,
resolution)
# Randomly choose an orientation for each particle between 0 and 2 pi
rand_orientation = math.radians(randint(0, 359))
part.append(rand_orientation)
# Add particle to our initial particle set
initial_particle_set.append(part)
# Initialize our particle cloud to be the size of our map
self.particle_cloud = []
# Use code from class to convert to particle type
for i in range(len(initial_particle_set)):
p = Pose()
p.position = Point()
p.position.x = initial_particle_set[i][0]
p.position.y = initial_particle_set[i][1]
p.position.z = 0
p.orientation = Quaternion()
q = quaternion_from_euler(0.0, 0.0, initial_particle_set[i][2])
p.orientation.x = q[0]
p.orientation.y = q[1]
p.orientation.z = q[2]
p.orientation.w = q[3]
# Initialize the new particle, where all will have the same weight (1.0)
new_particle = Particle(p, 1.0)
# Append the particle to the particle cloud
self.particle_cloud.append(new_particle)
# END
self.normalize_particles()
self.publish_particle_cloud()
def __init__(self):
rospy.init_node('nav_test', anonymous=False)
rospy.on_shutdown(self.shutdown)
# How big is the square we want the robot to navigate?
square_size = rospy.get_param("~square_size", 1.0) # meters
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi/2, pi, 3*pi/2, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
waypoints = self.createWaypoints()
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
waypoints.append(Pose(Point(0.054, 2, 0.0), quaternions[0]))
waypoints.append(Pose(Point(0.288, 1, 0.0), quaternions[1]))
waypoints.append(Pose(Point(-0.303, -0.303, 0.0), quaternions[2]))
waypoints.append(Pose(Point(-0.011, -0.033, 0.0), quaternions[3]))
# Initialize the visualization markers for RViz
self.init_markers()
# Set a visualization marker at each waypoint
for waypoint in waypoints:
p = Point()
p = waypoint.position
self.markers.points.append(p)
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_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))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the four waypoints
while not rospy.is_shutdown():
# Update the marker display
self.marker_pub.publish(self.markers)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'map'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = waypoints[i]
# Start the robot moving toward the goal
self.move(goal)
i += 1
i %= 4
def heading(self, yaw):
q = quaternion_from_euler(0, 0, yaw)
return Quaternion(*q)
p.pose.position.x = item_translation[0]
p.pose.position.y = item_translation[1]
p.pose.position.z = item_translation[2]
scene.add_box("target", p, (0.01, 0.01, 0.08))
# rospy.sleep(20)
# (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], - 0.080)
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "left_gripper_link"
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.position.x = aim_x
grasp_pose.pose.position.y = aim_y
grasp_pose.pose.position.z = aim_z
grasp_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0.0, 0, aim_yaw))
g = Grasp()
g.pre_grasp_posture = onine_arm.make_gripper_posture(0.09)
g.grasp_posture = onine_arm.make_gripper_posture(0.01)
g.pre_grasp_approach = onine_arm.make_gripper_translation(0.08, 0.10)
g.post_grasp_retreat = onine_arm.make_gripper_translation(0.08, 0.10, -1.0)
g.grasp_pose = grasp_pose
#2 degrees resolution
pitch_vals = [
-0.10472, -0.0698132, -0.0349066, 0, 0.0349066, 0.0698132, 0.10472
]
height_vals = [
-0.005, -0.004, -0.003, -0.002, -0.001, 0, 0.001, 0.002, 0.003, 0.004,
0.005
#!/usr/bin/env python
import sys, rospy, tf, actionlib
from geometry_msgs.msg import *
from tf.transformations import quaternion_from_euler
if __name__ == '__main__':
rospy.init_node('initial_localization')
pub = rospy.Publisher('initialpose',PoseWithCovarianceStamped,queue_size =1)
p = PoseWithCovarianceStamped()
p.header.frame_id ='map'
p.pose.pose.orientation = Quaternion(*quaternion_from_euler(0,0,0))
p.pose.covariance =[0.1 , 0, 0, 0, 0, 0,
0 , 0.1 , 0, 0, 0, 0,
0 , 0 , 0, 0, 0, 0,
0 , 0 , 0, 0, 0, 0,
0 , 0 , 0, 0, 0, 0,
0 , 0 , 0, 0, 0, 0.1]
for t in range(0,5):
rospy.sleep(1)
pub.publish(p)
def convert_xy_and_theta_to_pose(self, x, y, theta):
oriantation_tuple = t.quaternion_from_euler(0, 0, math.radians(theta))
position = (x, y, 0)
return self.convert_translation_rotation_to_pose(
position, oriantation_tuple)
def check_and_avoid(self, angle):
#print(self.front)
#print (self.right)
self.minfront = min(self.front)
self.minleft = min(self.left)
self.minright = min(self.right)
self.minback = min(self.back)
#print(self.minfront)
#print(self.minleft)
#print(self.minright)
target_alt = 40
if (self.minfront < 10 and self.minfront > .3):
if (self.avoidancepar == 1):
self.positionSp.pose.position.x = self.current_local_x
self.positionSp.pose.position.y = self.current_local_y
self.positionSp.pose.position.z = target_alt
rospy.loginfo("I saw it I will stop")
self.avoid_pub.publish(self.positionSp)
rospy.sleep(1.0)
self.avoidancepar = 2
if (self.minleft >= self.minright) and (
self.minleft > 10
or self.minleft < 0.3) and (self.avoidancepar == 2):
if (self.minfront < 10):
rospy.loginfo("Going Left")
self.positionSp.header.frame_id = 'fcu'
self.positionSp.pose.position.x = self.current_local_x
self.positionSp.pose.position.y = self.current_local_y + 20
self.positionSp.pose.position.z = target_alt
print(self.positionSp.pose.position)
print(self.positionSp.pose.orientation)
self.tf_listener.waitForTransform("/fcu", "/local_origin",
rospy.Time.now(),
rospy.Duration(10.0))
self.positionSp = self.tf_listener.transformPose(
"/local_origin", self.positionSp)
print("points to go to")
print(self.positionSp.pose.position)
print(self.positionSp.pose.orientation)
self.positionSp.pose.position.z = target_alt
self.positionSp.header.frame_id = 'local_origin'
yaw = angle
quaternion = quaternion_from_euler(0, 0, yaw)
self.positionSp.pose.orientation = Quaternion(*quaternion)
#self.positionSp2.position.x=self.pt_transformed
self.avoid_pub.publish(self.positionSp)
elif (self.minright >= self.minleft) and (
self.minright > 10
or self.minright < 0.3) and (self.avoidancepar == 2):
if (self.minfront < 10):
rospy.loginfo("Going Right")
self.positionSp.header.frame_id = 'fcu'
self.positionSp.pose.position.x = self.current_local_x
self.positionSp.pose.position.y = self.current_local_y - 20
self.positionSp.pose.position.z = target_alt
print(self.positionSp.pose.position.x)
print(self.positionSp.pose.position.y)
self.tf_listener.waitForTransform("/fcu", "/local_origin",
rospy.Time.now(),
rospy.Duration(10.0))
self.positionSp = self.tf_listener.transformPose(
"/local_origin", self.positionSp)
print("points to go to")
print(self.positionSp.pose.position.x)
print(self.positionSp.pose.position.y)
self.positionSp.pose.position.z = target_alt
self.positionSp.header.frame_id = 'local_origin'
yaw = angle
quaternion = quaternion_from_euler(0, 0, yaw)
self.positionSp.pose.orientation = Quaternion(*quaternion)
#self.positionSp2.position.x=self.pt_transformed
self.avoid_pub.publish(self.positionSp)
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:创建（调试）发布者：
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:创建规划场景和机器人命令：
self._scene = PlanningSceneInterface() #未知
self._robot = RobotCommander() #未知
rospy.sleep(0.5)
# Clean the scene:清理现场：
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:将桌子和可乐罐对象添加到计划场景：
self._pose_table = self._add_table(self._table_object_name) #增加桌子
self._pose_coke_can = self._add_grasp_block_(
self._grasp_object_name) #增加障碍物块
rospy.sleep(0.5)
# Define target place pose:定义目标位置姿势
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.20
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above
self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
self._arm.set_named_target('up')
self._arm.go(wait=True)
print("up pose")
print("第一部分恢复位置初始")
# Create grasp generator 'generate' action client:
# #创建抓取生成器“生成”动作客户端：
print("开始Action通信")
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
print("结束Action通信")
# Create move group 'pickup' action client:
# 创建移动组“抓取”操作客户端：
print("开始pickup通信")
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
print("结束pickup通信")
# Create move group 'place' action client:
# 创建移动组“放置”动作客户端：
print("开始place通信")
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
print("结束place通信")
# Pick Coke can object:抓取快
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(0.5)
print("抓取物体")
rospy.loginfo('Pick up successfully')
print("pose_place: ", self._pose_place)
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(0.5)
rospy.loginfo('Place successfully')
def LocateOffice(self):
rospy.loginfo("I'm leaving for an office now")
# Information to inform Arduino to turn put LEDs into 'driving around' state
self.led_state = 0
self.led.publish(self.led_state)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
# Reference the appropriate AWS service
dynamodb = boto3.resource(
'dynamodb',
aws_access_key_id=self.ACCESS_KEY,
aws_secret_access_key=self.SECRET_KEY,
)
table = dynamodb.Table('IDO_Studio')
# Get coordinates and angle for the desired office location
response = table.query(TableName='IDO_Studio',
KeyConditionExpression=Key('Office').eq(
self.locate_office_id))
x_coord = response[u'Items'][0][u'X_Coord']
y_coord = response[u'Items'][0][u'Y_Coord']
angle = response[u'Items'][0][u'Angle']
# Change locate office flag in database to False
locate_office_field = 'FALSE'
response = table.update_item(
Key={
'Office': self.locate_office_id,
},
UpdateExpression="set Locate_Office = :o",
ExpressionAttributeValues={':o': locate_office_field},
)
# Transform the robot angle to a quaternion for robot orientation
base_quat = transformations.quaternion_from_euler(
0, 0, math.radians(angle))
# Set the goal to have the coordinates and angle required to be near the docking station
goal.target_pose.pose = Pose(
Point(float(x_coord), float(y_coord), float(0)),
Quaternion(float(base_quat[0]), float(base_quat[1]),
float(base_quat[2]), float(base_quat[3])))
# Direct the robot to begin moving
self.move_base.send_goal(goal)
# Give the robot up to time to get close to the docking station
success = self.move_base.wait_for_result(
rospy.Duration(self.wait_locate_office))
if success:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("The robot made it to the identified office")
self.at_docking_station = False
self.WaitForNextRequest()
return True
else:
self.move_base.cancel_goal()
rospy.loginfo("The robot did not make it to the identified office")
self.error_flag = True
self.error_location = 'LocateOffice failed'
self.led_state = 6
self.led.publish(self.led_state)
self.at_docking_station = False
self.EscortUser()
return False
def set_pose(limb, kin, jointname, end_point, rpy,elbow, rl,mode):
"""
Controls the joints of this limb to the specified positions by using the end effector pose and elbow angle.
:param limb: (str:Limb) Interface class for a limb on the Baxter robot
:param kin: (str:Limb) baxter kinematics class
:param jointname: (dict({str})) - joint_name
:param end_point: (list({float})) - end effector positions
:param rpy: (list({float})) - orientations
:param elbow: float - elbow joint angles
:param rl: int - specifiy either left or right limb to be controlled ( 0 = right, 1 = left )
:param mode: int - mapping algorithm mode
:return: -
"""
global r_last_command_baxter_angles
global l_last_command_baxter_angles
if mode==1: #elbow lying in the horizontal axis and both end effectors are pointing toward each others
if rl==0:
quat = quaternion_from_euler (rpy[0]+pi/2,-rpy[2]+pi/2,rpy[1],axes='rzyz') #right
else:
quat = quaternion_from_euler (rpy[0]-pi/2,rpy[2]+pi/2,-rpy[1],axes='rzyz') #left
current_pose=limb.endpoint_pose()
position=current_pose['position']
orientation=current_pose['orientation']
#adding the ofset to map 2 different range of motions from exoskeleton and Baxter robot
if rl==0:
x=0.16+end_point[0]
y=(end_point[1]+0.2)*1.25-0.2
z=0.55+end_point[2]
else:
x=0.16+end_point[0]
y=(end_point[1]-0.2)*1.25+0.2
z=0.55+end_point[2]
o_x=quat[0]
o_y=quat[1]
o_z=quat[2]
o_w=quat[3]
new_pose=[x,y,z]
new_rot=[o_x,o_y,o_z,o_w]
if rl==0:
angles=kin.inverse_kinematics(new_pose,new_rot,r_last_command_baxter_angles) #,r_last_command_baxter_angles
else:
angles=kin.inverse_kinematics(new_pose,new_rot,l_last_command_baxter_angles)
jacob=kin.jacobian()
new_jacob=np.concatenate((jacob,[[0,0,1,0,0,0,0]]), axis=0)
jinv=np.linalg.inv(new_jacob)
# rotates the elbow of the Baxter to match the desired pose from exoskeleton
if rl==0:
speed_in_null_space=np.matrix([0,0,0,0,0,0,(elbow-angles[2]-pi/2)*0.5]) #right
else:
speed_in_null_space=np.matrix([0,0,0,0,0,0,(elbow-angles[2]+pi/2)*0.5]) #left
#get angular speed for rotating each joints to match the desired pose
angular_speed=np.matmul(jinv,speed_in_null_space.transpose())
new_angles=np.squeeze([(a+b) for a, b in zip(angles,angular_speed)])
elif mode==2: #elbow joints are pointing upward and the end effectors are pointing downward
if rl==0:
quat = quaternion_from_euler (rpy[0]-pi,-rpy[1],-rpy[2]+pi,axes='rzyx') #right
else:
quat = quaternion_from_euler (rpy[0]-pi,-rpy[1],-rpy[2]-pi,axes='rzyx') #left
current_pose=limb.endpoint_pose()
position=current_pose['position']
orientation=current_pose['orientation']
#adding the ofset to map 2 different range of motions from exoskeleton and Baxter robot
x=0.24+end_point[0]
y=end_point[1]
z=0.5+end_point[2]
o_x=quat[0]
o_y=quat[1]
o_z=quat[2]
o_w=quat[3]
new_pose=[x,y,z]
new_rot=[o_x,o_y,o_z,o_w]
if rl==0:
angles=kin.inverse_kinematics(new_pose,new_rot,r_last_command_baxter_angles) #,r_last_command_baxter_angles
else:
angles=kin.inverse_kinematics(new_pose,new_rot,l_last_command_baxter_angles)
jacob=kin.jacobian()
new_jacob=np.concatenate((jacob,[[0,0,1,0,0,0,0]]), axis=0)
jinv=np.linalg.inv(new_jacob)
# rotates the elbow of the Baxter to match the desired pose from exoskeleton
if rl==0:
speed_in_null_space=np.matrix([0,0,0,0,0,0,(elbow-angles[2]-pi*0.45)*0.8]) #right
else:
speed_in_null_space=np.matrix([0,0,0,0,0,0,(elbow-angles[2]+pi*0.45)*0.8]) #left
#get angular speed for rotating each joints to match the desired pose
angular_speed=np.matmul(jinv,speed_in_null_space.transpose())
new_angles=np.squeeze([(a+b) for a, b in zip(angles,angular_speed)])
if new_angles is not None:
if rl==0:
if all(i-j <= 1 and i-j >= -1 for i,j in zip(new_angles,r_last_command_baxter_angles)):
filtered_angles_r=[(a + 9*b)/10 for a, b in zip(new_angles,r_last_command_baxter_angles)]
set_j(limb,jointname,filtered_angles_r)
r_last_command_baxter_angles[:]=filtered_angles_r[:]
else:
if all(i-j <= 1 and i-j >= -1 for i,j in zip(new_angles,l_last_command_baxter_angles)):
filtered_angles_l=[(a + 9*b)/10 for a, b in zip(new_angles,l_last_command_baxter_angles)]
set_j(limb,jointname,filtered_angles_l)
l_last_command_baxter_angles[:]=filtered_angles_l[:]
robot.right_arm.go()
robot.right_gripper.set_joint_value_target(GRIPPER_OPEN)
robot.right_gripper.go()
rospy.sleep(5)
# pick pose
p = PoseStamped()
p.header.frame_id = "up1_footprint"
p.pose.position.x = 0.12792118579
p.pose.position.y = -0.285290879999
p.pose.position.z = 0.120301181892
roll = 0
pitch = 0
yaw = -1.57 #pi/2 radians
q = quaternion_from_euler(roll, pitch, yaw)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
robot.right_arm.set_pose_target(p.pose)
max_pick_attempts = 5
success = 0
attempt = 0
while success != 1 and attempt < max_pick_attempts:
p_plan = robot.right_arm.plan()
attempt = attempt + 1
print "Planning attempt: " + str(attempt)
if p_plan.joint_trajectory.points != []:
qua_w = input("Quaternion w: ")
qua_x = input("Quaternion x: ")
qua_y = input("Quaternion y: ")
qua_z = input("Quaternion z: ")
elif choice == 4:
crc.move_l_position()
elif choice == 5:
pos_x = input("Position x: ")
pos_y = input("Position y: ")
pos_z = input("Position z: ")
x = input("Euler x: ")
y = input("Euler y: ")
z = input("Euler z: ")
euler = [math.radians(x), math.radians(y), math.radians(z)]
from tf.transformations import quaternion_from_euler
quaternion = quaternion_from_euler(euler[0], euler[1], euler[2])
qua_x = quaternion[0]
qua_y = quaternion[1]
qua_z = quaternion[2]
qua_w = quaternion[3]
else:
rospy.logwarn("Enter only valid options")
if not choice == 4:
this_pose = deepcopy(pose)
this_pose.position.x = float(pos_x)
this_pose.position.y = float(pos_y)
this_pose.position.z = float(pos_z)
this_pose.orientation.w = float(qua_w)
this_pose.orientation.x = float(qua_x)
this_pose.orientation.y = float(qua_y)
def main():
global gazebo_model_states
OBJECT_NAME = "wood_cube_5cm" # 掴むオブジェクトの名前
GRIPPER_OPEN = 1.2 # 掴む時のハンド開閉角度
GRIPPER_CLOSE = 0.42 # 設置時のハンド開閉角度
APPROACH_Z = 0.15 # 接近時のハンドの高さ
LEAVE_Z = 0.20 # 離れる時のハンドの高さ
PICK_Z = 0.12 # 掴む時のハンドの高さ
PLACE_POSITIONS = [ # オブジェクトの設置位置 (ランダムに設置する)
Point(0.4, 0.0, 0.0),
Point(0.0, 0.3, 0.0),
Point(0.0, -0.3, 0.0),
Point(0.2, 0.2, 0.0),
Point(0.2, -0.2, 0.0)]
sub_model_states = rospy.Subscriber("gazebo/model_states", ModelStates, callback, queue_size=1)
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.4)
arm.set_max_acceleration_scaling_factor(1.0)
gripper = actionlib.SimpleActionClient("crane_x7/gripper_controller/gripper_cmd", GripperCommandAction)
gripper.wait_for_server()
gripper_goal = GripperCommandGoal()
gripper_goal.command.max_effort = 4.0
rospy.sleep(1.0)
while True:
# 何かを掴んでいた時のためにハンドを開く
gripper_goal.command.position = GRIPPER_OPEN
gripper.send_goal(gripper_goal)
gripper.wait_for_result(rospy.Duration(1.0))
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
rospy.sleep(1.0)
# 一定時間待機する
# この間に、ユーザがgazebo上のオブジェクト姿勢を変更しても良い
sleep_time = 3.0
print("Wait " + str(sleep_time) + " secs.")
rospy.sleep(sleep_time)
print("Start")
# オブジェクトがgazebo上に存在すれば、pick_and_placeを実行する
if OBJECT_NAME in gazebo_model_states.name:
object_index = gazebo_model_states.name.index(OBJECT_NAME)
# オブジェクトの姿勢を取得
object_position = gazebo_model_states.pose[object_index].position
object_orientation = gazebo_model_states.pose[object_index].orientation
object_yaw = yaw_of(object_orientation)
# オブジェクトに接近する
target_pose = Pose()
target_pose.position.x = object_position.x
target_pose.position.y = object_position.y
target_pose.position.z = APPROACH_Z
q = quaternion_from_euler(-math.pi, 0.0, object_yaw)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
if arm.go() is False:
print("Failed to approach an object.")
continue
rospy.sleep(1.0)
# 掴みに行く
target_pose.position.z = PICK_Z
arm.set_pose_target(target_pose)
if arm.go() is False:
print("Failed to grip an object.")
continue
rospy.sleep(1.0)
gripper_goal.command.position = GRIPPER_CLOSE
gripper.send_goal(gripper_goal)
gripper.wait_for_result(rospy.Duration(1.0))
# 持ち上げる
target_pose.position.z = LEAVE_Z
arm.set_pose_target(target_pose)
if arm.go() is False:
print("Failed to pick up an object.")
continue
rospy.sleep(1.0)
# 設置位置に移動する
place_position = random.choice(PLACE_POSITIONS) # 設置位置をランダムに選択する
target_pose.position.x = place_position.x
target_pose.position.y = place_position.y
q = quaternion_from_euler(-math.pi, 0.0, -math.pi/2.0)
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose)
if arm.go() is False:
print("Failed to approach target position.")
continue
rospy.sleep(1.0)
# 設置する
target_pose.position.z = PICK_Z
arm.set_pose_target(target_pose)
if arm.go() is False:
print("Failed to place an object.")
continue
rospy.sleep(1.0)
gripper_goal.command.position = GRIPPER_OPEN
gripper.send_goal(gripper_goal)
gripper.wait_for_result(rospy.Duration(1.0))
# ハンドを上げる
target_pose.position.z = LEAVE_Z
arm.set_pose_target(target_pose)
if arm.go() is False:
print("Failed to leave from an object.")
continue
rospy.sleep(1.0)
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
if arm.go() is False:
print("Failed to go back to home pose.")
continue
rospy.sleep(1.0)
print("Done")
else:
print("No objects")
def add_sentence(self, nmea_string, frame_id, timestamp=None):
if not check_nmea_checksum(nmea_string):
rospy.logwarn("Received a sentence with an invalid checksum. " +
"Sentence was: %s" % repr(nmea_string))
return False
parsed_sentence = libnmea_navsat_driver.parser.parse_nmea_sentence(
nmea_string)
if not parsed_sentence:
rospy.logdebug("Failed to parse NMEA sentence. Sentence was: %s" %
nmea_string)
return False
if timestamp:
current_time = timestamp
else:
current_time = rospy.get_rostime()
current_fix = NavSatFix()
current_fix.header.stamp = current_time
current_fix.header.frame_id = frame_id
current_time_ref = TimeReference()
current_time_ref.header.stamp = current_time
current_time_ref.header.frame_id = frame_id
if self.time_ref_source:
current_time_ref.source = self.time_ref_source
else:
current_time_ref.source = frame_id
if not self.use_RMC and 'GGA' in parsed_sentence:
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_APPROXIMATED
data = parsed_sentence['GGA']
fix_type = data['fix_type']
if not (fix_type in self.gps_qualities):
fix_type = -1
gps_qual = self.gps_qualities[fix_type]
default_epe = gps_qual[0]
current_fix.status.status = gps_qual[1]
current_fix.position_covariance_type = gps_qual[2]
if gps_qual > 0:
self.valid_fix = True
else:
self.valid_fix = False
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
# Altitude is above ellipsoid, so adjust for mean-sea-level
altitude = data['altitude'] + data['mean_sea_level']
current_fix.altitude = altitude
# use default epe std_dev unless we've received a GST sentence with epes
if not self.using_receiver_epe or math.isnan(self.lon_std_dev):
self.lon_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.lat_std_dev):
self.lat_std_dev = default_epe
if not self.using_receiver_epe or math.isnan(self.alt_std_dev):
self.alt_std_dev = default_epe * 2
hdop = data['hdop']
current_fix.position_covariance[0] = (hdop * self.lon_std_dev)**2
current_fix.position_covariance[4] = (hdop * self.lat_std_dev)**2
current_fix.position_covariance[8] = (2 * hdop *
self.alt_std_dev)**2 # FIXME
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.last_valid_fix_time = current_time_ref
self.time_ref_pub.publish(current_time_ref)
elif not self.use_RMC and 'VTG' in parsed_sentence:
data = parsed_sentence['VTG']
# Only report VTG data when you've received a valid GGA fix as well.
if self.valid_fix:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'RMC' in parsed_sentence:
data = parsed_sentence['RMC']
# Only publish a fix from RMC if the use_RMC flag is set.
if self.use_RMC:
if data['fix_valid']:
current_fix.status.status = NavSatStatus.STATUS_FIX
else:
current_fix.status.status = NavSatStatus.STATUS_NO_FIX
current_fix.status.service = NavSatStatus.SERVICE_GPS
latitude = data['latitude']
if data['latitude_direction'] == 'S':
latitude = -latitude
current_fix.latitude = latitude
longitude = data['longitude']
if data['longitude_direction'] == 'W':
longitude = -longitude
current_fix.longitude = longitude
current_fix.altitude = float('NaN')
current_fix.position_covariance_type = \
NavSatFix.COVARIANCE_TYPE_UNKNOWN
self.fix_pub.publish(current_fix)
if not math.isnan(data['utc_time']):
current_time_ref.time_ref = rospy.Time.from_sec(
data['utc_time'])
self.time_ref_pub.publish(current_time_ref)
# Publish velocity from RMC regardless, since GGA doesn't provide it.
if data['fix_valid']:
current_vel = TwistStamped()
current_vel.header.stamp = current_time
current_vel.header.frame_id = frame_id
current_vel.twist.linear.x = data['speed'] * \
math.sin(data['true_course'])
current_vel.twist.linear.y = data['speed'] * \
math.cos(data['true_course'])
self.vel_pub.publish(current_vel)
elif 'GST' in parsed_sentence:
data = parsed_sentence['GST']
# Use receiver-provided error estimate if available
self.using_receiver_epe = True
self.lon_std_dev = data['lon_std_dev']
self.lat_std_dev = data['lat_std_dev']
self.alt_std_dev = data['alt_std_dev']
elif 'HDT' in parsed_sentence:
data = parsed_sentence['HDT']
if data['heading']:
current_heading = QuaternionStamped()
current_heading.header.stamp = current_time
current_heading.header.frame_id = frame_id
q = quaternion_from_euler(0, 0, math.radians(data['heading']))
current_heading.quaternion.x = q[0]
current_heading.quaternion.y = q[1]
current_heading.quaternion.z = q[2]
current_heading.quaternion.w = q[3]
self.heading_pub.publish(current_heading)
else:
return False
try:
rospy.wait_for_service('/gazebo/set_link_state', 1)
except rospy.ROSException as e:
print("Error %s" % e)
sys.exit(1)
rospy.Subscriber('/setup/fisheye/image_raw',
Image,
image_callback,
queue_size=10)
img_counter = 0
t = np.array([0.3, 0, 0.45])
rpy = np.array(np.deg2rad([0, 0, 0]))
change_link_state(t, quaternion_from_euler(*rpy))
rospack = rospkg.RosPack()
pkg_path = rospack.get_path('ocam-calibration')
# subscriber is working in another thread
# here we use blocking stdin for chessboard pose control
while not rospy.is_shutdown():
key = sys.stdin.read(1)
if key == '\n':
continue # skip 'Enter' key
if key in t_step_key_map:
t += t_step_key_map[key]
change_link_state(t, quaternion_from_euler(*rpy))
def createRotationFootStepList(self, yaw):
left_footstep = FootstepDataRosMessage()
left_footstep.robot_side = FootstepDataRosMessage.LEFT
right_footstep = FootstepDataRosMessage()
right_footstep.robot_side = FootstepDataRosMessage.RIGHT
left_foot_world = self.tfBuffer.lookup_transform(
'world', self.LEFT_FOOT_FRAME_NAME, rospy.Time())
right_foot_world = self.tfBuffer.lookup_transform(
'world', self.RIGHT_FOOT_FRAME_NAME, rospy.Time())
intermediate_transform = Transform()
# create a virtual fram between the feet, this will be the center of the rotation
intermediate_transform.translation.x = (
left_foot_world.transform.translation.x +
right_foot_world.transform.translation.x) / 2.
intermediate_transform.translation.y = (
left_foot_world.transform.translation.y +
right_foot_world.transform.translation.y) / 2.
intermediate_transform.translation.z = (
left_foot_world.transform.translation.z +
right_foot_world.transform.translation.z) / 2.
# here we assume that feet have the same orientation so we can pick arbitrary left or right
intermediate_transform.rotation = left_foot_world.transform.rotation
left_footstep.location = left_foot_world.transform.translation
right_footstep.location = right_foot_world.transform.translation
# define the turning radius
radius = sqrt((right_foot_world.transform.translation.x -
left_foot_world.transform.translation.x)**2 +
(right_foot_world.transform.translation.y -
left_foot_world.transform.translation.y)**2) / 2.
left_offset = [-radius * sin(yaw), radius * (1 - cos(yaw)), 0]
right_offset = [radius * sin(yaw), -radius * (1 - cos(yaw)), 0]
intermediate_euler = euler_from_quaternion([
intermediate_transform.rotation.x,
intermediate_transform.rotation.y,
intermediate_transform.rotation.z,
intermediate_transform.rotation.w
])
resulting_quat = quaternion_from_euler(intermediate_euler[0],
intermediate_euler[1],
intermediate_euler[2] + yaw)
rot = quaternion_matrix([
resulting_quat[0], resulting_quat[1], resulting_quat[2],
resulting_quat[3]
])
left_transformedOffset = numpy.dot(rot[0:3, 0:3], left_offset)
right_transformedOffset = numpy.dot(rot[0:3, 0:3], right_offset)
quat_final = Quaternion(resulting_quat[0], resulting_quat[1],
resulting_quat[2], resulting_quat[3])
left_footstep.location.x += left_transformedOffset[0]
left_footstep.location.y += left_transformedOffset[1]
left_footstep.location.z += left_transformedOffset[2]
left_footstep.orientation = quat_final
right_footstep.location.x += right_transformedOffset[0]
right_footstep.location.y += right_transformedOffset[1]
right_footstep.location.z += right_transformedOffset[2]
right_footstep.orientation = quat_final
if yaw > 0:
return [left_footstep, right_footstep]
else:
return [right_footstep, left_footstep]
def test_setTargetPoseRelative_rpy(self):
'''
Test if with setTargetPoseRelative with RPY values the arm pose becomes as intended.
Contributed by Naoki Fuse (Daido Steel).
'''
print "goInitial", self.robot.getCurrentRPY('RARM_JOINT5'), self.robot.getCurrentRPY('LARM_JOINT5')
l_eef = 'LARM_JOINT5'
r_eef = 'RARM_JOINT5'
init_l = quaternion_from_euler(-3.073, -1.57002, 3.073) # goInit (not factory setting) pose
init_r = quaternion_from_euler(3.073, -1.57002, -3.073) # goInit (not factory setting) pose
roll_l_post = quaternion_from_euler(-1.502, -1.5700, 3.073) # dr=math.pi / 2 from init pose
roll_r_post = quaternion_from_euler(-1.639, -1.5700, -3.073) # dr=math.pi / 2 from init pose
pitch_l_post = quaternion_from_euler(-0.000, -0.787, -4.924e-05) # dp=math.pi / 4 from init pose
pitch_r_post = quaternion_from_euler(0.000, -0.787, 4.827e-05) # dp=math.pi / 4 from init pose
yaw_l_post = quaternion_from_euler(-1.573, -4.205e-05, 1.571) # dw=math.pi / 2 from init pose
yaw_r_post = quaternion_from_euler(-1.573, -0.000, 1.571) # dw=math.pi / 2 from init pose
# roll motion
self.robot.goInitial(2)
for i in range(0, 5): # Repeat the same movement 5 times
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dr=math.pi / 2, tm=0.5, wait=False)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=math.pi / 2, tm=0.5, wait=True)
roll_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
roll_l_post_now = quaternion_from_euler(roll_l_post_now_rpy[0], roll_l_post_now_rpy[1], roll_l_post_now_rpy[2])
roll_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
roll_r_post_now = quaternion_from_euler(roll_r_post_now_rpy[0], roll_r_post_now_rpy[1], roll_r_post_now_rpy[2])
numpy.testing.assert_array_almost_equal(roll_l_post, roll_l_post_now, decimal=2)
numpy.testing.assert_array_almost_equal(roll_r_post, roll_r_post_now, decimal=2)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dr=-math.pi / 2, dw=0, tm=0.5, wait=False)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dr=-math.pi / 2, dw=0, tm=0.5, wait=True)
init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
# pitch motion
self.robot.goInitial(2)
for i in range(0, 5):
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dp=math.pi / 4, tm=0.5, wait=False)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=math.pi / 4, tm=0.5, wait=True)
pitch_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
pitch_l_post_now = quaternion_from_euler(pitch_l_post_now_rpy[0], pitch_l_post_now_rpy[1], pitch_l_post_now_rpy[2])
pitch_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
pitch_r_post_now = quaternion_from_euler(pitch_r_post_now_rpy[0], pitch_r_post_now_rpy[1], pitch_r_post_now_rpy[2])
numpy.testing.assert_array_almost_equal(pitch_l_post, pitch_l_post_now, decimal=2)
numpy.testing.assert_array_almost_equal(pitch_r_post, pitch_r_post_now, decimal=2)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dp=-math.pi / 4, dw=0, tm=0.5, wait=False)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dp=-math.pi / 4, dw=0, tm=0.5, wait=True)
init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
# yaw motion
self.robot.goInitial(2)
for i in range(0, 5):
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dw=math.pi / 2, tm=0.5, wait=False)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=math.pi / 2, tm=0.5, wait=True)
yaw_l_post_now_rpy = self.robot.getCurrentRPY(l_eef)
yaw_l_post_now = quaternion_from_euler(yaw_l_post_now_rpy[0], yaw_l_post_now_rpy[1], yaw_l_post_now_rpy[2])
yaw_r_post_now_rpy = self.robot.getCurrentRPY(r_eef)
yaw_r_post_now = quaternion_from_euler(yaw_r_post_now_rpy[0], yaw_r_post_now_rpy[1], yaw_r_post_now_rpy[2])
numpy.testing.assert_array_almost_equal(yaw_l_post, yaw_l_post_now, decimal=2)
numpy.testing.assert_array_almost_equal(yaw_r_post, yaw_r_post_now, decimal=2)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_LEFT_ARM, l_eef, dw=-math.pi / 2, tm=0.5, wait=False)
self.robot.setTargetPoseRelative(RTM_JOINTGRP_RIGHT_ARM, r_eef, dw=-math.pi / 2, tm=0.5, wait=True)
init_l_now_rpy = self.robot.getCurrentRPY(l_eef)
init_l_now = quaternion_from_euler(init_l_now_rpy[0], init_l_now_rpy[1], init_l_now_rpy[2])
init_r_now_rpy = self.robot.getCurrentRPY(r_eef)
init_r_now = quaternion_from_euler(init_r_now_rpy[0], init_r_now_rpy[1], init_r_now_rpy[2])
numpy.testing.assert_array_almost_equal(init_l, init_l_now, decimal=2)
numpy.testing.assert_array_almost_equal(init_r, init_r_now, decimal=2)
def odom_callback(odommsg):
# print "odom callback"
global nav_err, La, gps_var, seq, gps_msg_old, lidar_msg_old, odom_msg_old, tmp_msg, alpha
global gl_GPS_offsetX, gl_GPS_offsetY
lidar_msg = [0.0] * 6
odom_msg = [0.0] * 6
gps_fix_msg = list(gps_msg)
gps_fix_msg[0] = gps_msg[0] + gl_GPS_offsetX
gps_fix_msg[1] = gps_msg[1] + gl_GPS_offsetY
try:
(trans, rot) = tf_listener.lookupTransform('/map', '/base_footprint',
rospy.Time(0))
# print trans
# print rot
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return 1
# print("3")
(lidar_roll, lidar_pitch, lidar_yaw) = euler_from_quaternion(rot)
lidar_msg[0:3] = trans
lidar_msg[3] = lidar_roll
lidar_msg[4] = lidar_pitch
lidar_msg[5] = lidar_yaw
odom_msg[0] = odommsg.pose.pose.position.x
odom_msg[1] = odommsg.pose.pose.position.y
odom_msg[2] = odommsg.pose.pose.position.z
orientation = [
odommsg.pose.pose.orientation.x, odommsg.pose.pose.orientation.y,
odommsg.pose.pose.orientation.z, odommsg.pose.pose.orientation.w
]
(odom_roll, odom_pitch, odom_yaw) = euler_from_quaternion(orientation)
odom_msg[3] = odom_roll
odom_msg[4] = odom_pitch
odom_msg[5] = odom_yaw
if gps_get_flag == 1:
(nav_flag, delta2comb) = nav_status(gps_fix_msg, lidar_msg, odom_msg,
gps_msg_old, lidar_msg_old,
odom_msg_old, tmp_msg)
# print gps_msg_old
## Status GPS Lidar Return ###############
# 0 0 0
# 0 1 1
# 1 0 2
# 1 1 3
############################################
print(nav_flag)
global count
if nav_flag == 2:
count += 1
# print("~~~~~~~~~~~~~~",count,"~~~~~~~~~~~~~~~")
# while(1):
# a = 1
#
if nav_flag == 3 or nav_flag == 1:
out_msg = list(gps_fix_msg)
tmp_msg = list(gps_fix_msg)
La = 0
elif nav_flag == 2:
out_msg = list(lidar_msg)
tmp_msg = list(lidar_msg)
gl_GPS_offsetX = lidar_msg[0] - gps_msg[0]
gl_GPS_offsetY = lidar_msg[1] - gps_msg[1]
La = 0
else:
nav_err += 1
La += delta2comb
tmp_msg[0] += (odom_msg[0] - odom_msg_old[0])
tmp_msg[1] += (odom_msg[1] - odom_msg_old[1])
tmp_msg[2] += (odom_msg[2] - odom_msg_old[2])
tmp_msg[3] = odom_msg[3]
tmp_msg[4] = odom_msg[4]
tmp_msg[5] = odom_msg[5]
gl_GPS_offsetX = lidar_msg[0] - gps_msg[0]
gl_GPS_offsetY = lidar_msg[1] - gps_msg[1]
# print ("gps_fix", gps_fix_msg)
# print ('gps_msg', gps_msg)
# print ('odom_msg', odom_msg)
# print ('lidar_msg', lidar_msg)
# print ('tmp_msg', tmp_msg)
gps_msg_old = list(gps_msg)
lidar_msg_old = list(lidar_msg)
odom_msg_old = list(odom_msg)
return 0
fix_pos.header.frame_id = "map"
fix_pos.pose.pose.position.x = out_msg[0]
fix_pos.pose.pose.position.y = out_msg[1]
fix_pos.pose.pose.position.z = out_msg[2]
q = quaternion_from_euler(out_msg[3], out_msg[4], out_msg[5])
fix_pos.pose.pose.orientation.x = q[0]
fix_pos.pose.pose.orientation.y = q[1]
fix_pos.pose.pose.orientation.z = q[2]
fix_pos.pose.pose.orientation.w = q[3]
fix_pos.header.seq = seq
seq = seq + 1
print("gps_fix", gps_fix_msg)
print('gps_msg', gps_msg)
print('odom_msg', odom_msg)
print('lidar_msg', lidar_msg)
print('tmp_msg', tmp_msg)
print('out_msg', out_msg)
pub.publish(fix_pos)
# /map-/base_footprint
map_af_trans = out_msg[0:3]
map_af_quat = q
# /laser_map-/base_footprint
lidar_af_trans = lidar_msg[0:3]
lidar_af_quat = rot
# /odom-/base_footprint
odom_af_trans = odom_msg[0:3]
odom_af_quat = orientation
map_af_mat44 = np.dot(transformations.translation_matrix(map_af_trans),
transformations.quaternion_matrix(map_af_quat))
af_map_mat44 = np.linalg.inv(map_af_mat44)
lidar_af_mat44 = np.dot(
transformations.translation_matrix(lidar_af_trans),
transformations.quaternion_matrix(lidar_af_quat))
# txpose is the new pose in target_frame as a 4x4
txpose = np.dot(lidar_af_mat44, af_map_mat44)
# xyz and quat are txpose's position and orientation
txpose = np.linalg.inv(txpose)
tran_af_mat44 = np.dot(
transformations.translation_matrix([0, 0, 0]),
transformations.quaternion_matrix(
quaternion_from_euler(0, 0, alpha)))
txpose_fix = np.dot(tran_af_mat44, txpose)
xyz = tuple(transformations.translation_from_matrix(txpose_fix))[:3]
quat = tuple(transformations.quaternion_from_matrix(txpose_fix))
br = tf.TransformBroadcaster()
br.sendTransform(xyz, quat, rospy.Time.now(), "map", "world")
gps_msg_old = list(gps_msg)
lidar_msg_old = list(lidar_msg)
odom_msg_old = list(odom_msg)
def reset(self, model_state=None, id_bots=999):
'''
Resettng the Experiment
1. Update the counter based on the flag from step
2. Assign next positions and reset the positions of robots and targets
'''
# ========================================================================= #
# 1. TARGET UPDATE #
# ========================================================================= #
self.is_collided = False
tb3_0 = -1
tb3_1 = -1
tb3_2 = -1
tb3_3 = -1
if model_state is not None:
for i in range(len(model_state.name)):
if model_state.name[i] == 'tb3_0':
tb3_0 = i
if model_state.name[i] == 'tb3_1':
tb3_1 = i
if model_state.name[i] == 'tb3_2':
tb3_2 = i
if model_state.name[i] == 'tb3_3':
tb3_3 = i
else:
tb3_0 = -1
tb3_1 = -2
tb3_2 = -3
tb3_3 = -4
if id_bots == 999:
if self.target_index < self.num_experiments - 1:
self.target_index += 1
else:
self.target_index = 0
angle_target = self.target_index * 2 * pi / self.num_experiments
self.x[0] = (self.d_robots / 2) * cos(angle_target)
self.y[0] = (self.d_robots / 2) * sin(angle_target)
self.x[1] = (self.d_robots / 2) * cos(angle_target + pi)
self.y[1] = (self.d_robots / 2) * sin(angle_target + pi)
self.x[2] = (self.d_robots / 2) * cos(angle_target + pi / 2)
self.y[2] = (self.d_robots / 2) * sin(angle_target + pi / 2)
self.x[3] = (self.d_robots / 2) * cos(angle_target + 3 * pi / 2)
self.y[3] = (self.d_robots / 2) * sin(angle_target + 3 * pi / 2)
self.theta[0] = angle_target + pi
self.theta[1] = angle_target
self.theta[2] = angle_target + 3 * pi / 2
self.theta[3] = angle_target + pi / 2
quat1 = quaternion_from_euler(0, 0, self.theta[0])
quat2 = quaternion_from_euler(0, 0, self.theta[1])
quat3 = quaternion_from_euler(0, 0, self.theta[2])
quat4 = quaternion_from_euler(0, 0, self.theta[3])
self.target = [[self.x[1], self.y[1]], [self.x[0], self.y[0]],
[self.x[3], self.y[3]], [self.x[2], self.y[2]]]
# ========================================================================= #
# 2. RESET #
# ========================================================================= #
# Reset Turtlebot 1 position
state_msg = ModelState()
state_msg.model_name = 'tb3_0'
state_msg.pose.position.x = self.x[0]
state_msg.pose.position.y = self.y[0]
state_msg.pose.position.z = 0.0
state_msg.pose.orientation.x = quat1[0]
state_msg.pose.orientation.y = quat1[1]
state_msg.pose.orientation.z = quat1[2]
state_msg.pose.orientation.w = quat1[3]
# Reset Turtlebot 2 Position
state_msg2 = ModelState()
state_msg2.model_name = 'tb3_1' #'unit_sphere_0_0' #'unit_box_1' #'cube_20k_0'
state_msg2.pose.position.x = self.x[1]
state_msg2.pose.position.y = self.y[1]
state_msg2.pose.position.z = 0.0
state_msg2.pose.orientation.x = quat2[0]
state_msg2.pose.orientation.y = quat2[1]
state_msg2.pose.orientation.z = quat2[2]
state_msg2.pose.orientation.w = quat2[3]
# Reset Turtlebot 3 Position
state_msg3 = ModelState()
state_msg3.model_name = 'tb3_2' #'unit_sphere_0_0' #'unit_box_1' #'cube_20k_0'
state_msg3.pose.position.x = self.x[2]
state_msg3.pose.position.y = self.y[2]
state_msg3.pose.position.z = 0.0
state_msg3.pose.orientation.x = quat3[0]
state_msg3.pose.orientation.y = quat3[1]
state_msg3.pose.orientation.z = quat3[2]
state_msg3.pose.orientation.w = quat3[3]
# Reset Turtlebot 4 Position
state_msg4 = ModelState()
state_msg4.model_name = 'tb3_3' #'unit_sphere_0_0' #'unit_box_1' #'cube_20k_0'
state_msg4.pose.position.x = self.x[3]
state_msg4.pose.position.y = self.y[3]
state_msg4.pose.position.z = 0.0
state_msg4.pose.orientation.x = quat4[0]
state_msg4.pose.orientation.y = quat4[1]
state_msg4.pose.orientation.z = quat4[2]
state_msg4.pose.orientation.w = quat4[3]
rospy.wait_for_service('gazebo/reset_simulation')
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
if id_bots == 999 or id_bots == tb3_0:
resp = set_state(state_msg)
self.dones[0] = False
if id_bots == 999 or id_bots == tb3_1:
resp2 = set_state(state_msg2)
self.dones[1] = False
if id_bots == 999 or id_bots == tb3_2:
resp3 = set_state(state_msg3)
self.dones[2] = False
if id_bots == 999 or id_bots == tb3_3:
resp4 = set_state(state_msg4)
self.dones[3] = False
except rospy.ServiceException as e:
print("Service Call Failed: %s" % e)
initial_state = np.zeros((self.num_robots, self.state_num))
self.just_reset = True
self.move_cmd.linear.x = 0.0
self.move_cmd.angular.z = 0.0
if id_bots == 999 or id_bots == tb3_0:
self.pub_tb3_0.publish(self.move_cmd)
if id_bots == 999 or id_bots == tb3_1:
self.pub_tb3_1.publish(self.move_cmd)
if id_bots == 999 or id_bots == tb3_2:
self.pub_tb3_2.publish(self.move_cmd)
if id_bots == 999 or id_bots == tb3_3:
self.pub_tb3_3.publish(self.move_cmd)
self.rate.sleep()
rospy.wait_for_service('phero_reset')
try:
phero_reset = rospy.ServiceProxy('phero_reset', PheroReset)
resp = phero_reset(True)
print("Reset Pheromone grid successfully: {}".format(resp))
except rospy.ServiceException as e:
print("Service Failed %s" % e)
#self.dones = [False] * self.num_robots
return range(0, self.num_robots), initial_state
def optimizeManeuver(self, req):
'''
Sets up the optimization problem then calculates the optimal maneuver
poses. Runs when any message is sent to the corresponding subscriber,
the 'msg' value does not matter.
Args:
-----
msg: ROS Bool message
Returns:
--------
path: the optimal path as a ROS nav_msgs/Path message
'''
# Make sure a target pose exists
if (self.target_x is not None):
# DEBUG:
print("Running maneuver optimization...")
# Set initial guess
x_s = self.target_x + np.cos(
self.target_approach_angle) * self.approach_pose_offset
y_s = self.target_y + np.sin(
self.target_approach_angle) * self.approach_pose_offset
theta_s = self.target_approach_angle # the starting pose is facing backwards, so the approach angle is the same as the starting orientation rather than adding 180deg
r_1 = 2
alpha_1 = -np.pi / 2
r_2 = 2
alpha_2 = np.pi / 2
# # DEBUG: Print starting path for optimizer
# pose_s = Pose2D(x_s, y_s, theta_s)
# [pose_m, pose_f] = self.maneuverPoses(pose_s, r_1, alpha_1, abs(r_2), abs(alpha_2)) # this function expects r_2 and alpha_2 to be positve values
#
# path = self.maneuverSegmentPath(pose_s, r_1, alpha_1)
# path.extend(self.maneuverSegmentPath(pose_m, r_2*-np.sign(r_1), alpha_2*-np.sign(alpha_1)))
# self.maneuver_path.header.stamp = rospy.Time.now()
# self.maneuver_path.poses = []
# for i in range(len(path)):
# point = PoseStamped()
# point.pose.position.x = path[i][0]
# point.pose.position.y = path[i][1]
# self.maneuver_path.poses.append(point)
#
# self.path_pub.publish(self.maneuver_path)
# raw_input("Pause")
x0 = [x_s, y_s, theta_s, r_1, alpha_1, r_2, alpha_2]
# Set params
# TODO:
# add the forklifts current pose from "/odom"
current_pose2D = Pose2D()
current_pose2D.x = self.current_pose.position.x
current_pose2D.y = self.current_pose.position.y
euler_angles = euler_from_quaternion([
self.current_pose.orientation.x,
self.current_pose.orientation.y,
self.current_pose.orientation.z,
self.current_pose.orientation.w
])
current_pose2D.theta = euler_angles[2]
params = {
"current_pose":
[current_pose2D.x, current_pose2D.y, current_pose2D.theta],
"forklift_length": (self.base_to_back + self.base_to_clamp),
"weights": [10, 1, 0.1, 1],
"obstacles":
self.obstacles,
"min_radius":
self.min_radius
}
# Set up optimization problem
obj = lambda x: self.maneuverObjective(x, params)
ineq_con = {
'type': 'ineq',
'fun': lambda x: self.maneuverIneqConstraints(x, params),
'jac': None
}
bounds = [(-20, 20), (-20, 20), (-np.pi, np.pi),
(-10 * np.pi, 10 * np.pi), (-np.pi, np.pi),
(self.min_radius, 10 * np.pi), (np.pi / 4, np.pi)]
# Optimize
res = minimize(obj,
x0,
method='SLSQP',
bounds=bounds,
constraints=ineq_con)
# DEBUG:
print("===== Optimization Results =====")
print("Success: %s" % res.success)
print("Message: %s" % res.message)
print(
"Results:\n x: %f, y: %f, theta: %f\n r_1: %f, alpha_1: %f\n r_2: %f, alpha_2: %f"
% (res.x[0], res.x[1], res.x[2], res.x[3], res.x[4], res.x[5],
res.x[6]))
# Store result
x_s = res.x[0]
y_s = res.x[1]
theta_s = res.x[2]
r_1 = res.x[3]
alpha_1 = res.x[4]
r_2 = res.x[5]
alpha_2 = res.x[6]
# NOTE: remember to make the sign of r_2 opposite of r_1 and alpha_2 opposite of alpha_1
r_2 = -np.sign(r_1) * r_2
alpha_2 = -np.sign(alpha_1) * alpha_2
pose_s = Pose2D(x_s, y_s, theta_s)
[pose_m, pose_f] = self.maneuverPoses(
pose_s, r_1, alpha_1, abs(r_2), abs(alpha_2)
) # this function expects r_2 and alpha_2 to be positve values
# Publish first segment of maneuver
path1 = self.maneuverSegmentPath(pose_s, r_1, alpha_1)
self.maneuver_path.path.header.stamp = rospy.Time.now()
self.maneuver_path.path.poses = []
for i in range(len(path1)):
point = PoseStamped()
point.pose.position.x = path1[i][0]
point.pose.position.y = path1[i][1]
self.maneuver_path.path.poses.append(point)
# Set gear, positive alpha = forward gear
self.maneuver_path.gear = np.sign(alpha_1)
self.path1_pub.publish(self.maneuver_path)
# Publish second segment of maneuver
path2 = self.maneuverSegmentPath(pose_m, r_2, alpha_2)
self.maneuver_path.path.poses = []
for i in range(len(path2)):
point = PoseStamped()
point.pose.position.x = path2[i][0]
point.pose.position.y = path2[i][1]
self.maneuver_path.path.poses.append(point)
# Set gear, positive alpha = forward gear
self.maneuver_path.gear = np.sign(alpha_2)
self.path2_pub.publish(self.maneuver_path)
self.optimization_success = res.success
if (self.optimization_success):
# If optimization was successful, publish the new target
# position for the A* algorithm (you will want to make this a
# separate "goal" value distinct from the roll target position)
rospy.set_param("/control_panel_node/goal_x", float(pose_s.x))
rospy.set_param("/control_panel_node/goal_y", float(pose_s.y))
# Publish the starting pose for the approach b-spline path
obstacle_end_pose = PoseStamped()
obstacle_end_pose.header.frame_id = "/odom"
obstacle_end_pose.pose.position.x = pose_f.x
obstacle_end_pose.pose.position.y = pose_f.y
quat_forklift = quaternion_from_euler(0, 0,
wrapToPi(pose_f.theta))
obstacle_end_pose.pose.orientation.x = quat_forklift[0]
obstacle_end_pose.pose.orientation.y = quat_forklift[1]
obstacle_end_pose.pose.orientation.z = quat_forklift[2]
obstacle_end_pose.pose.orientation.w = quat_forklift[3]
self.approach_pose_pub.publish(obstacle_end_pose)
return OptimizeManeuverResponse(self.optimization_success)
else:
return OptimizeManeuverResponse(False)
def heading(yaw): """A helper function to getnerate quaternions from yaws.""" q = quaternion_from_euler(0, 0, yaw) return Quaternion(*q)
while not rospy.is_shutdown():
print("ekf'in")
yk[0, 0] = x
yk[1, 0] = y
yk[2, 0] = (rwheelAccum - rwheelAccumPrev) / h
yk[3, 0] = (lwheelAccum - lwheelAccumPrev) / h
mk, Pk = my_ekf.filter_update(yk, mk, Pk)
pose_point.header.frame_id = "/map"
pose_point.pose.position.x = mk[0, 0]
pose_point.pose.position.y = mk[1, 0]
(pose_point.pose.orientation.x, pose_point.pose.orientation.y, pose_point.pose.orientation.z, pose_point.pose.orientation.w) = \
quaternion_from_euler(0.0, 0.0, mk[2,0])
print("THETA", mk[2, 0])
print("x-estimate: ", pose_point.pose.position.x)
print("y-estimate: ", pose_point.pose.position.y)
#m[k] = mk
#P[k] = Pk
pub_ekf.publish(pose_point)
lwheelAccumPrev = lwheelAccum
rwheelAccumPrev = rwheelAccum
rate.sleep()
def build_steps(self, forward, lateral, turn):
L = self.params["Forward Stride Length"]["value"]
L_lat = self.params["Lateral Stride Length"]["value"]
R = self.params["Turn Radius"]["value"]
W = self.params["Stride Width"]["value"]
X = 0
Y = 0
theta = 0
dTheta = 0
if forward != 0:
dTheta = turn * 2 * math.asin(L / (2 * (R + \
self.params["Stride Width"]["value"]/2)))
else:
dTheta = turn * self.params["In Place Turn Size"]["value"]
steps = []
if forward != 0:
dTheta = turn * 2 * math.asin(L / (2 * (R + \
self.params["Stride Width"]["value"]/2)))
else:
dTheta = turn * self.params["In Place Turn Size"]["value"]
steps = []
# This home step doesn't currently do anything, but it's a
# response to bdi not visiting the first step in a trajectory
# home_step = AtlasBehaviorStepData()
# If moving right, first dummy step is on the left
# home_step.foot_index = 1*(lateral < 0)
# home_step.pose.position.y = 0.1
# steps.append(home_step)
prevX = 0
prevY = 0
# Builds the sequence of steps needed
for i in range(self.params["Walk Sequence Length"]["value"]):
# is_right_foot = 1, when stepping with right
is_even = i % 2
is_odd = 1 - is_even
is_right_foot = is_even
is_left_foot = is_odd
# left = 1, right = -1
foot = 1 - 2 * is_right_foot
if self.is_static:
theta = (turn != 0) * dTheta
if turn == 0:
X = (forward != 0) * (forward * L)
Y = (lateral != 0) * (is_odd * lateral * L_lat) + \
foot * W / 2
elif forward != 0:
# Radius from point to foot (if turning)
R_foot = R + foot * W / 2
# turn > 0 for CCW, turn < 0 for CW
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot * math.cos(theta))
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
elif turn != 0:
X = turn * W / 2 * math.sin(theta)
Y = turn * W / 2 * math.cos(theta)
else:
theta += (turn != 0) * dTheta
if turn == 0:
X = (forward != 0) * (X + forward * L)
Y = (lateral != 0) * (Y + is_odd * lateral * L_lat) + \
foot * W / 2
elif forward != 0:
# Radius from point to foot (if turning)
R_foot = R + foot * W / 2
# turn > 0 for CCW, turn < 0 for CW
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot * math.cos(theta))
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
elif turn != 0:
X = turn * W / 2 * math.sin(theta)
Y = turn * W / 2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
# One step already exists, so add one to index
step.step_index = i
# Alternate between feet, start with left
step.foot_index = is_right_foot
#If moving laterally to the left, start with the right foot
if (lateral > 0):
step.foot_index = is_left_foot
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
steps.append(step)
# Add final step to bring feet together
is_right_foot = 1 - steps[-1].foot_index
is_even = is_right_foot
# foot = 1 for left, foot = -1 for right
foot = 1 - 2 * is_right_foot
if turn == 0:
Y = Y + foot * W
elif forward != 0:
self.debuginfo("R: " + str(R) + " R_foot:" + \
str(R_foot) + " theta: " + str(theta) + \
" math.sin(theta): " + str(math.sin(theta)) + \
" math.cos(theta) + " + str(math.cos(theta)))
# R_foot is radius to foot
R_foot = R + foot * W / 2
#turn > 0 for counter clockwise
X = forward * turn * R_foot * math.sin(theta)
Y = forward * turn * (R - R_foot * math.cos(theta))
else:
X = turn * W / 2 * math.sin(theta)
Y = turn * W / 2 * math.cos(theta)
Q = quaternion_from_euler(0, 0, theta)
step = AtlasBehaviorStepData()
step.step_index = len(steps)
step.foot_index = is_right_foot
step.duration = self.params["Stride Duration"]["value"]
step.pose.position.x = X
step.pose.position.y = Y
step.pose.position.z = self.params["Step Height"]["value"]
step.pose.orientation.x = Q[0]
step.pose.orientation.y = Q[1]
step.pose.orientation.z = Q[2]
step.pose.orientation.w = Q[3]
step.swing_height = self.params["Swing Height"]["value"]
steps.append(step)
return steps
