def createArea(frame):
poly = PolygonStamped()
poly.header.frame_id = frame
poly.header.stamp = rospy.Time.now()
poly.polygon = Polygon()
poly.polygon.points = exploration_area_vertices
return poly
def main():
rospy.init_node('kobuki_exploration')
area_to_explore = PolygonStamped()
center_point = PointStamped()
now = rospy.Time.now()
area_to_explore.header.stamp = now
area_to_explore.header.frame_id = "map"
points = [
Point32(-2.65, -2.56, 0.0),
Point32(5.41, -2.7, 0.0),
Point32(5.57, 4.44, 0.0),
Point32(-2.75, 4.37, 0.0)
]
area_to_explore.polygon = Polygon(points)
center_point.header = area_to_explore.header
center_point.point.x = 1.83
center_point.point.y = 1.57
center_point.point.z = 0.0
brain = PR2RobotBrain(area_to_explore, center_point)
brain.getReady()
brain.loop()
def main():
rospy.init_node('pr2_state_machine')
brain = PR2RobotBrain()
brain.getReady()
area_to_explore = PolygonStamped()
center_point = PointStamped()
now = rospy.Time.now()
area_to_explore.header.stamp = now
area_to_explore.header.frame_id = "map"
points = [Point32(-1.65, -1.56, 0.0),
Point32(5.41, -1.7, 0.0),
Point32(5.57, 4.44, 0.0),
Point32(-1.75, 4.37, 0.0)]
area_to_explore.polygon = Polygon(points)
center_point.header = area_to_explore.header
center_point.point.x = 1.83
center_point.point.y = 1.57
center_point.point.z = 0.0
brain = PR2RobotBrain(area_to_explore, center_point)
brain.loop()
def pose_callback(self, pose_msg):
'''
Uses a pose message to generate an odometry and state message.
:param pose_msg: (geometry_msgs/PoseStamped) pose message
'''
new_pose_msg, trans, rot = self.tf_to_pose("LO01_base_link", "map",
pose_msg.header)
if not self.use_amcl:
pose_msg = new_pose_msg
self.new_pose_pub.publish(
pose_msg
) # if using AMCL, this will be the same as the original pose message
if trans and rot: # if not getting tf, trans and rot will be None
footprint = PolygonStamped()
footprint.header = pose_msg.header # has same frame_id (map) and time stamp
loomo_points = np.array([[0.16, -0.31], [0.16, 0.31],
[-0.16, 0.31], [-0.16, -0.31]])
roll, pitch, yaw = tf.transformations.euler_from_quaternion(rot)
rot = np.array([[np.cos(yaw), -np.sin(yaw)],
[np.sin(yaw), np.cos(yaw)]])
rotated_points = np.matmul(rot, loomo_points.T) # 2x4 array
rot_and_trans = rotated_points + np.array([[trans[0]], [trans[1]]])
polygon = Polygon(
points=[Point32(x=x, y=y, z=0) for x, y in rot_and_trans.T])
footprint.polygon = polygon
self.footprint_pub.publish(footprint)
odom_msg = Odometry()
odom_msg.pose.pose = pose_msg.pose
odom_msg.header = pose_msg.header
self.odom_pub.publish(odom_msg)
state_msg = State()
state_msg.header = pose_msg.header
state_msg.state_stamp = pose_msg.header.stamp
state_msg.pos.x = pose_msg.pose.position.x
state_msg.pos.y = pose_msg.pose.position.y
state_msg.pos.z = pose_msg.pose.position.z
state_msg.quat.x = pose_msg.pose.orientation.x
state_msg.quat.y = pose_msg.pose.orientation.y
state_msg.quat.z = pose_msg.pose.orientation.z
state_msg.quat.w = pose_msg.pose.orientation.w
if self.last_state_time and self.last_state:
dt = pose_msg.header.stamp.nsecs - self.last_state_time
state_msg.vel.x = (state_msg.pos.x -
self.last_state.pos.x) / (float(dt) / 10**9)
state_msg.vel.y = (state_msg.pos.y -
self.last_state.pos.y) / (float(dt) / 10**9)
state_msg.vel.z = 0 # ground robot not traveling in z direction
self.last_state_time = pose_msg.header.stamp.nsecs
self.last_state = state_msg
self.state_pub.publish(state_msg)
def polygon_cb(msg_var, data_var):
global polygon_voronoi_publisher, header
idx = data_var
polygon_msg = PolygonStamped()
polygon_msg.header = header
polygon_msg.polygon = msg_var
#print data_var,msg_var
polygon_voronoi_publisher[idx].publish(polygon_msg)
def spin(self):
# base convex hull
hull = self.build_base_hull(self.footprint)
# add points to the hull
now = rospy.Time.now()
try:
for target_frame in self.target_frames:
joint = PointStamped()
joint.header.frame_id = target_frame
joint.header.stamp = now
# transform
self.tf_listener.waitForTransform(target_frame,
self.footprint_frame, now,
rospy.Duration(1))
tf_joint = self.tf_listener.transformPoint(
self.footprint_frame, joint)
# append joint and its inflated version
r = self.inflation_radius
np_joint = np.array([
(tf_joint.point.x, tf_joint.point.y),
(tf_joint.point.x + r, tf_joint.point.y + r),
(tf_joint.point.x + r, tf_joint.point.y - r),
(tf_joint.point.x - r, tf_joint.point.y + r),
(tf_joint.point.x - r, tf_joint.point.y - r)
])
hull.add_points(np_joint)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
pass
except tf.Exception:
rospy.logwarn("TF Exception. Possible clock reset.")
hull.close()
# generate and publish Polygon, PolygonStamped
polygon_msg = self.build_polygon_msg(hull)
polygon_stamped_msg = PolygonStamped()
polygon_stamped_msg.header.frame_id = self.footprint_frame
polygon_stamped_msg.header.stamp = now
polygon_stamped_msg.polygon = polygon_msg
self.pub.publish(polygon_msg)
self.pub_stamped.publish(polygon_stamped_msg)
# publish base footprint
self.base_msg.header.stamp = now
self.pub_original_stamped.publish(self.base_msg)
def publish_triangle_polygon(self, pose):
pts = []
pts.append(tfx.pose((0,0.0079,0)))
pts.append(tfx.pose((0,-0.0079,0)))
pts.append(tfx.pose((0,0, -0.013)))
for i in range(len(pts)):
# IPython.embed()
pts[i] = (pose.as_tf()*pts[i]).position.msg.Point()
polygon = Polygon()
polygon_stamped = PolygonStamped()
polygon.points = pts
polygon_stamped.polygon = polygon
polygon_stamped.header = pose.msg.Header()
self.triangle_polygon_publisher.publish(polygon_stamped)
def convert_shapely_as_ros_polygon(shapely_polygon):
polygon_stamped = PolygonStamped()
polygon_stamped.header.seq = 1
polygon_stamped.header.stamp = rospy.Time.now()
polygon_stamped.header.frame_id = "/map"
ros_polygon = RosPolygon()
coords = list(shapely_polygon.exterior.coords)
for coord in coords:
point = Point32()
point.x = coord[0]
point.y = coord[1]
ros_polygon.points.append(point)
polygon_stamped.polygon = ros_polygon
return polygon_stamped
def main():
# Create the publisher, which is using the desired output topic and message
pub = rospy.Publisher('detections', PolygonStamped, queue_size=10)
# Initialize our node, which we will call task1
rospy.init_node('task1', anonymous=True)
# publish at a rate of 1Hz
rate = rospy.Rate(1) # 1hz
seq = 0;
# a loop which iterates through as long as the ros server is online
while not rospy.is_shutdown():
# we use a sin function to make the object move around
# polygon stamped is a type of ros message which allows you to be able to represent a polygon with a coordinate frame and time stamp
msg = PolygonStamped();
# polygon is a ros message which allows you to specify a polygon based on where the initial and final points are assumed to be connected
detection = Polygon();
# take the first point, and create a 'box' around it using rounding
t1 = rospy.Time.now();
# puts a time stamp at t1 - as the initial point
msg.header.stamp = t1;
# increment the sequence from last time
seq += 1;
msg.header.seq = seq;
# catvehicle/odom is the odometry topic you are subscribing to
msg.header.frame_id = 'catvehicle/odom'
# sinusoidal functions create a circular path and each point is then converted to a square
# the generation of square is done below through ceil and floor functions of x and y points
x = 3*math.sin(0.2*t1.to_sec()); #to_sec converts t1 into seconds
y = 3*math.cos(0.2*t1.to_sec()); # creating a circle with respect to time
z = 0.1;
# appending 4 different x y z coordinates to basically build a polygon frame
detection.points.append(Point(floor(x),floor(y),z));
detection.points.append(Point(ceil(x),floor(y),z));
detection.points.append(Point(ceil(x),ceil(y),z));
detection.points.append(Point(floor(x),ceil(y),z));
# then this polygon is then converted into a polgon message
msg.polygon = detection;
# writing loginto into stdout
rospy.loginfo(msg);
# publishing the ros message with the polygon information
pub.publish(msg);
# rospy.rate is specified to be 1 hz so by doing rate.sleep() we are making ROS sleep for 1 second
rate.sleep()
def deflect_path(self):
# use the nearest colliding point to deflect the path accordingly
if self.closest_pt_index is not None:
dist = self.laser_ranges[self.closest_pt_index]
angle = normalize_angle(
self.real_laser_angles[self.closest_pt_index] +
self.FORWARD_ANGLE)
angle_rel_car_pose = self.laser_angles[self.closest_pt_index]
print("angle", angle)
print("best", self.best_pt_angle)
deflection_dir = 1 if (sign(self.best_pt_angle) < sign(angle) or
(sign(self.best_pt_angle) == sign(angle) and
self.best_pt_angle - angle > 0)) else -1
deflection_factor = deflection_dir * min(
(self.MAX_TURN_ANGLE) / (max(dist, 1.0)**2) *
abs(abs(angle_rel_car_pose) - math.pi / 4) /
(math.pi / 4), self.MAX_TURN_ANGLE)
print("def factor", deflection_factor)
new_angle = normalize_angle(self.best_pt_angle + deflection_factor)
print("new angle", new_angle)
self.deflected_pt = (self.cur_pose.position.x +
self.best_pt_dist * math.cos(new_angle),
self.cur_pose.position.y +
self.best_pt_dist * math.sin(new_angle))
else:
self.deflected_pt = self.best_pt
path = PolygonStamped()
path.header = Utils.make_header("map")
path.polygon = Polygon([
Point(self.cur_pose.position.x, self.cur_pose.position.y, 0),
Point(self.deflected_pt[0], self.deflected_pt[1], 0)
])
self.path_command_pub.publish(path)
def publish_line(self):
# find the two points that intersect between the fan angle lines and the found y=mx+c line
x0 = self.c / (np.tan(FAN_ANGLE) - self.m)
x1 = self.c / (-np.tan(FAN_ANGLE) - self.m)
y0 = self.m*x0+self.c
y1 = self.m*x1+self.c
poly = Polygon()
p0 = Point32()
p0.y = x0
p0.x = y0
p1 = Point32()
p1.y = x1
p1.x = y1
poly.points.append(p0)
poly.points.append(p1)
polyStamped = PolygonStamped()
polyStamped.header.frame_id = "base_link"
polyStamped.polygon = poly
self.line_pub.publish(polyStamped)
def callback(msg):
if msg.camera_id not in frame_pubs:
frame_pubs[msg.camera_id] = rospy.Publisher(
'/roomba_detection_frame_marker/{}'.format(msg.camera_id),
PolygonStamped,
queue_size=5)
frame = PolygonStamped()
frame.header = msg.header
frame.polygon = msg.detection_region
frame_pubs[msg.camera_id].publish(frame)
marker_arr = MarkerArray()
for roomba in msg.roombas:
marker = Marker()
marker.header = msg.header
marker.ns = 'roomba_observations'
global count
marker.id = count
count += 1
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.scale.x = 0.05
if roomba.box_uncertainty < 0.3:
marker.scale.y = roomba.box_uncertainty
else:
marker.scale.y = 0.3
marker.scale.z = 0.0
if roomba.box_uncertainty > 0.3:
marker.color.r = 1
marker.color.g = 0
marker.color.b = 1
marker.color.a = 1
elif roomba.flip_certainty > 0.5:
marker.color.r = 0
marker.color.g = 1
marker.color.b = 1
marker.color.a = 1
else:
marker.color.r = 1
marker.color.g = 1
marker.color.b = 0
marker.color.a = 1
marker.lifetime = rospy.Duration(0.5)
marker.frame_locked = False
point = Point()
point.x = roomba.pose.x
point.y = roomba.pose.y
point.z = 0
marker.points.append(point)
point = Point()
point.x = roomba.pose.x + 0.2 * math.cos(roomba.pose.theta)
point.y = roomba.pose.y + 0.2 * math.sin(roomba.pose.theta)
point.z = 0
marker.points.append(point)
marker_arr.markers.append(marker)
vis_pub.publish(marker_arr)
def PublishPolygon(self):
polygonStamped = PolygonStamped()
polygonStamped.header.stamp = rospy.Time().now()
polygonStamped.header.frame_id = self.base_frame
polygonStamped.polygon = self.polygon
self.polygon_pub.publish(polygonStamped)
def callback(data):
#init some important parameters
r = []
count = 0
angular = []
sigma = 0.25 #this parameter denotes the sensitivity of the sensor
obj_num = 1
obj_num2 = 0
seq = 0
num_points = len(data.ranges)
r_index_divide = []
pi = 3.1415926
#filt all the points within 10 meters of the sensor
rospy.loginfo(data)
for x in data.ranges:
count += 1
if x < 10:
r.append(x)
angular.append(count)
pass
pass
#this algorithm combine some adjacent points to the same object
for r_index in range(len(r) - 1):
if r[r_index] - r[r_index + 1] < -sigma or r[r_index] - r[r_index +
1] > sigma:
obj_num += 1
pass
pass
#divide our laser data into different objects
r_divide = [[] for i in range(obj_num)]
angular_divide = [[] for i in range(obj_num)]
r_divide[0].append(r[0])
angular_divide[0].append(angular[0])
for r_index in range(len(r) - 1):
if r[r_index] - r[r_index + 1] >= -sigma and r[r_index] - r[
r_index + 1] <= sigma:
r_divide[obj_num2].append(r[r_index + 1])
angular_divide[obj_num2].append(angular[r_index + 1])
else:
obj_num2 += 1
r_divide[obj_num2].append(r[r_index + 1])
angular_divide[obj_num2].append(angular[r_index + 1])
#publish all the polygons
for i in range(obj_num):
msg = PolygonStamped()
detection = Polygon()
t1 = rospy.Time.now()
# puts a time stamp at t1 - as the initial point
msg.header.stamp = t1
seq += 1
msg.header.seq = seq
# catvehicle/odom is the odometry topic you are subscribing to
msg.header.frame_id = 'catvehicle/odom'
x = []
y = []
z = 0.1
#convert the polar coordinates to Catesian coordinates
for j in range(len(r_divide[i])):
x.append(r_divide[i][j] *
cos(angular_divide[i][j] * pi / num_points - pi / 2) +
2.4)
y.append(r_divide[i][j] *
sin(angular_divide[i][j] * pi / num_points - pi / 2))
pass
# here we use the bounding box strategy to represent the barrier
x_pose = cat_pose.x
y_pose = cat_pose.y
detection.points.append(Point(min(x) + x_pose,
min(y) + y_pose, z))
detection.points.append(Point(max(x) + x_pose,
min(y) + y_pose, z))
detection.points.append(Point(max(x) + x_pose,
max(y) + y_pose, z))
detection.points.append(Point(min(x) + x_pose,
max(y) + y_pose, z))
# then this polygon is then converted into a polgon message
msg.polygon = detection
# writing loginto into stdout
rospy.loginfo(msg)
# publishing the ros message with the polygon information
pub = rospy.Publisher('detections', PolygonStamped, queue_size=10)
rate = rospy.Rate(120)
pub.publish(msg)
# pub2 = rospy.Publisher('catvehicle/cmd_vel', Twist, queue_size=10)
# # rate = rospy.Rate(1)
# msg2=Twist()
# v_linear=Vector3()
# v_angular=Vector3()
# v_linear.x=2
# v_linear.y=0
# v_linear.z=0
# v_angular.x=0
# v_angular.y=0
# v_angular.z=0
# msg2.linear=v_linear
# msg2.angular=v_angular
# rospy.loginfo(msg2)
# pub2.publish(msg2)
rate.sleep()
pass
def execute_gcode(self, x_offset, y_offset, z_offset, simulation=False):
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("svenzva_arm")
path_pose_publisher = rospy.Publisher("/drawing/gcode_path",
PolygonStamped,
queue_size=2)
target_pose_publisher = rospy.Publisher("/target_pose",
PoseStamped,
queue_size=2)
if not simulation:
action_client = actionlib.SimpleActionClient(
'revel/follow_joint_trajectory',
control_msgs.msg.FollowJointTrajectoryAction)
action_client.wait_for_server()
rospy.wait_for_service('compute_ik')
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
ik_req = moveit_msgs.msg.PositionIKRequest()
ik_req.group_name = "svenzva_arm"
ik_req.ik_link_name = "base_link"
first_z = True
first_z_pos = 0.05
last_point = Point()
delta_copy = self.delta_pos
gcode_ar = self.stuff_gcode()
poly_stmp_msg = PolygonStamped()
poly_stmp_msg.header.frame_id = "base_link"
poly_msg = Polygon()
poly_stmp_msg.polygon = poly_msg
i = 0
while not rospy.is_shutdown() and i < len(gcode_ar):
cur_rob_pose = group.get_current_pose().pose
ik_req.robot_state = robot.get_current_state()
z_stall_condition = False
z_delta = 0.0
res = gcode_ar[i]
i += 1
gcode_pose = last_point
if res.x != "":
gcode_pose.x = float(res.x + x_offset)
if res.y != "":
gcode_pose.y = float(res.y + y_offset)
if res.z != "":
first_z = False
z_delta = abs(gcode_pose.z - res.z - z_offset)
print z_delta
gcode_pose.z = float(res.z + z_offset)
if first_z:
gcode_pose.z = first_z_pos
#begin display publishing
poly_point = Point32()
poly_point.x = gcode_pose.x
poly_point.y = gcode_pose.y
poly_point.z = gcode_pose.z
poly_msg.points.append(poly_point)
last_point = gcode_pose
pub_pose = PoseStamped()
pub_pose.header.frame_id = "base_link"
pub_pose.pose.orientation = self.maintain_orientation()
pub_pose.pose.position = gcode_pose
path_pose_publisher.publish(poly_stmp_msg)
path_pose_publisher.publish(poly_stmp_msg)
target_pose_publisher.publish(pub_pose)
target_pose_publisher.publish(pub_pose)
#end display publishing
if len(ik_req.pose_stamped_vector) > 0:
ik_req.pose_stamped_vector.pop()
ik_req.pose_stamped_vector.append(pub_pose)
try:
resp = compute_ik(ik_req)
pos = resp.solution.joint_state.position
goal = SvenzvaJointGoal()
goal.positions = resp.solution.joint_state.position
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5',
'joint_6'
]
point = JointTrajectoryPoint()
point.positions = resp.solution.joint_state.position
#Since this is asynchronous, the time from 2 points is 0 and the action will return immediately
point.time_from_start = rospy.Duration(0.1)
goal.trajectory.points.append(point)
if not simulation:
action_client.send_goal(goal)
# For Z-positional movements about this value, reduce the delta_pos controller parameter
# this sets the z_stall_condition flag which forces the robot to move to its commanded Z height
# fully before continuing to other points
if z_delta > 0.002:
z_stall_condition = True
self.delta_pos = 0.065
if simulation:
rospy.sleep(0.1)
else:
self.wait_for_stall(resp.solution.joint_state.position)
if z_stall_condition:
self.delta_pos = delta_copy
rospy.sleep(1.0)
except rospy.ServiceException, e:
rospy.loginfo("Service call failed: %s" % e)
return
from boun_affordance_service.srv import * from geometry_msgs.msg import PolygonStamped from geometry_msgs.msg import Polygon from geometry_msgs.msg import Point32 from geometry_msgs.msg import PoseArray from geometry_msgs.msg import Pose from geometry_msgs.msg import Point from geometry_msgs.msg import Quaternion from boun_affordance_service.msg import Affordance from boun_affordance_service.msg import AffordanceList import tf pcb_stamped = PolygonStamped() hdd_stamped = PolygonStamped() hdd_stamped.polygon = Polygon() #p1 = Point32(0, 0, 0) #p2 = Point32(0, 2, 0) #p3 = Point32(2, 2, 0) #p4 = Point32(2, 0, 0) #hdd_stamped.polygon.points = [p1, p2, p3, p4] pcb_stamped = PolygonStamped() #p1 = Point32(0.1, 0.1, 0) #p2 = Point32(0.1, 1, 0) #p3 = Point32(1.8, 1, 0) #p4 = Point32(1.8, 0.1, 0) #pcb_stamped.polygon.points = [p1, p2, p3, p4] leverup_poses = PoseArray() calculated_affordances = AffordanceList()
