def calc_position(self, markers_detected): global markers msg = AlvarMarkers() pose_array = PoseArray() #put here the prediction for multiple markers for marker in markers_detected: #print marker if not self.check_marker(marker['name']): self.create_new_marker(marker) pose = marker['pose'] name = marker['name'] quat = quat_msg_to_array(pose.pose.orientation) r, p, theta = tf.transformations.euler_from_quaternion(quat) q = tf.transformations.quaternion_from_euler(0, 0, theta) pose.pose.position.z = 0 pose.pose.orientation = Quaternion(*q) pose = self.predict_pose(markers[name]['kalman'], pose) markers[name]['last_seen'] = rospy.Time.now() marker_msg = AlvarMarker() marker_msg.pose = pose marker_msg.header = pose.header marker_msg.id = int(marker['name']) msg.markers.append(marker_msg) pose_array.poses.append(pose.pose) pose_array.header.stamp = rospy.Time.now() pose_array.header.frame_id = odom self.array_pub.publish(pose_array) msg.header.stamp = rospy.Time.now() msg.header.frame_id = odom self.marker_pub.publish(msg)
def publish(markers): ar_list = AlvarMarkers() for m in markers: ar = AlvarMarker() ar.id = m['id'] ar.confidence = m['confidence'] ar.pose.header.frame_id = m['frame_id'] ar.pose.header.stamp = rospy.Time.now() ar.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', m['pose']) ar_list.markers.append(ar) ar_pub.publish(ar_list) return
def read(filename): yaml_data = None with open(filename) as f: yaml_data = yaml.load(f) ar_list = AlvarMarkers() for m in yaml_data: ar = AlvarMarker() ar.id = m['id'] ar.confidence = m['confidence'] ar.pose.header.frame_id = m['frame_id'] ar.pose.header.stamp = rospy.Time.now() ar.pose.pose = message_converter.convert_dictionary_to_ros_message( 'geometry_msgs/Pose', m['pose']) ar_list.markers.append(ar) return ar_list
def publish(self): seq = 0 while not rospy.is_shutdown(): markers = AlvarMarkers() markers.header.stamp = rospy.get_rostime() markers.header.seq = seq markers.header.frame_id = _frame_id for name in self.objs.keys(): if name == 'COCA-COLA-CAN-250ML': self.objs[name].position.z += 0.08 if name in self.objDict.keys(): ################################ # publish AR marker ################################ marker = AlvarMarker() marker.id = self.objDict[name] marker.pose.pose = self.objs[name] marker.confidence = 1 marker.header.frame_id = _frame_id markers.markers.append(marker) ################################ # publish tf ################################ translation = (self.objs[name].position.x, self.objs[name].position.y, self.objs[name].position.z) rotation = (self.objs[name].orientation.x, self.objs[name].orientation.y, self.objs[name].orientation.z, self.objs[name].orientation.w) self._tf_broadcaster.sendTransform(translation, rotation, markers.header.stamp, name, _frame_id) ################################ # publish rviz marker ################################ rviz_marker = Marker() rviz_marker.header = markers.header rviz_marker.id = marker.id rviz_marker.pose = self.objs[name] rviz_marker.lifetime = rospy.Duration(1.0) rviz_marker.scale.x = 0.03 rviz_marker.scale.y = 0.03 rviz_marker.scale.z = 0.015 rviz_marker.ns = "basic_shapes" rviz_marker.type = Marker.CUBE rviz_marker.action = Marker.ADD if rviz_marker.id == 1: # red rviz_marker.color.r = 1.0 rviz_marker.color.g = 0.0 rviz_marker.color.b = 0.0 rviz_marker.color.a = 1.0 elif rviz_marker.id == 2: # blue rviz_marker.color.r = 0.0 rviz_marker.color.g = 0.0 rviz_marker.color.b = 1.0 rviz_marker.color.a = 1.0 else: # green rviz_marker.color.r = 0.0 rviz_marker.color.g = 1.0 rviz_marker.color.b = 0.0 rviz_marker.color.a = 1.0 self._visualPub.publish(rviz_marker) self._pub.publish(markers) seq = seq + 1 # no spinning needed in rospy rospy.sleep(1 / _pub_freq)