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()