def _cloud_cb(self, cloud): points = np.array(list(read_points(cloud, skip_nans=True))) if points.shape[0] == 0: return # Get 4x4 matrix which transforms point cloud co-ords to odometry frame try: points_to_map = self.tf.asMatrix('/odom', cloud.header) except tf.ExtrapolationException: return transformed_points = points_to_map.dot(np.vstack((points.T, np.ones((1, points.shape[0]))))) transformed_points = transformed_points[:3,:].T if self.fused is None: self.fused = transformed_points else: self.fused = np.vstack((self.fused, transformed_points)) self.seq += 1 header = Header() header.seq = self.seq header.stamp = rospy.Time.now() header.frame_id = '/odom' output_cloud = create_cloud(header, cloud.fields, self.fused) rospy.loginfo('Publishing new cloud') self.cloud_pub.publish(output_cloud)
def talker(): #Initial Pose Set Up pub_init = rospy.Publisher('initialpose', PoseWithCovarianceStamped) rospy.init_node('Initial_Pose_Publish') initial=PoseWithCovarianceStamped() initial_pose=PoseWithCovariance() initial_pose.pose=Pose(Point(0.100,0.100,0.100),Quaternion(0.000,0.000,-0.0149,0.999)) initial_pose.covariance=\ [1.0,0.0,0.0,0.0,0.0,0.0,\ 0.0,1.0,0.0,0.0,0.0,0.0,\ 0.0,0.0,1.0,0.0,0.0,0.0,\ 0.0,0.0,0.0,1.0,0.0,0.0,\ 0.0,0.0,0.0,0.0,1.0,0.0,\ 0.0,0.0,0.0,0.0,0.0,1.0] initial.pose=initial_pose initial_info=Header() initial_info.frame_id="map" initial_info.stamp=rospy.Time.now() initial.header=initial_info pub_init.publish(initial) r=rospy.Rate(1) ans=str("y") #publishing the goal while not rospy.is_shutdown(): rospy.loginfo("Setting Initial Pose") pub_init.publish(initial) ans=raw_input("Enter to continue") rospy.loginfo("Setting Goal")
def __init__(self): #Setup subscriber for combined searched image frontierMarkerSub = rospy.Subscriber('frontierMarkers',Marker,self.markerCallback,queue_size = 1) #Get Number of Robots self.numRobots = rospy.get_param('/num_robots') #Initialize Goal Publishers self.goalPub = [] for i in range(0, self.numRobots): #Setup Subscribers to individual robot searched grids topicName = 'robot_' + `i` +'/move_base_simple/goal' self.goalPub.append(rospy.Publisher(topicName,PoseStamped)) #Setup start/stop time publishers self.startPub = rospy.Publisher('startTime', Header) self.stopPub = rospy.Publisher('stopTime', Header) #Get simulation start time rospy.sleep(0.01) #Without this line, get_rostime always returns 0 self.start_time = rospy.get_rostime() rospy.loginfo("SIMULATION STARTED AT: %i %i", self.start_time.secs, self.start_time.nsecs) #Publish Start TIme startMsg = Header() startMsg.stamp = self.start_time startMsg.frame_id = '0' self.startPub.publish(startMsg) #Wait for subscribed messages to start arriving rospy.spin()
def create_move_group_pose_goal(self, goal_pose=Pose(), group="manipulator", end_link_name=None, plan_only=True): header = Header() header.frame_id = 'torso_base_link' header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() position_c = PositionConstraint() position_c.header = header if end_link_name != None: position_c.link_name = end_link_name position_c.constraint_region.primitives.append(SolidPrimitive(type=SolidPrimitive.SPHERE, dimensions=[0.01])) position_c.constraint_region.primitive_poses.append(goal_pose) position_c.weight = 1.0 goal_c.position_constraints.append(position_c) orientation_c = OrientationConstraint() orientation_c.header = header if end_link_name != None: orientation_c.link_name = end_link_name orientation_c.orientation = goal_pose.orientation orientation_c.absolute_x_axis_tolerance = 0.01 orientation_c.absolute_y_axis_tolerance = 0.01 orientation_c.absolute_z_axis_tolerance = 0.01 orientation_c.weight = 1.0 goal_c.orientation_constraints.append(orientation_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 5 moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True # Necessary moveit_goal.request.group_name = group return moveit_goal
def createRandomGrasps(grasp_pose, num_grasps=1): """ Returns a list of num_grasps Grasp's around grasp_pose """ list_grasps = [] for grasp_num in range(num_grasps): my_pre_grasp_approach = createGripperTranslation(Vector3(1.0, 0.0, 0.0)) # rotate over here? my_post_grasp_retreat = createGripperTranslation(Vector3(0.0, 0.0, 1.0)) header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() grasp_pose_copy = copy.deepcopy(grasp_pose) modified_grasp_pose = PoseStamped(header, grasp_pose_copy) #modified_grasp_pose.pose.position.x -= random.random() * 0.25 # randomize entrance to grasp in 25cm in x #modified_grasp_pose.pose.position.y -= random.random() * 0.10 # randomize entrance to grasp in 10cm in y #modified_grasp_pose.pose.orientation.z = -1.0 grasp = createGrasp(modified_grasp_pose, # change grasp pose? allowed_touch_objects=["part"], pre_grasp_posture=getPreGraspPosture(), grasp_posture=getPreGraspPosture(), pre_grasp_approach=my_pre_grasp_approach, post_grasp_retreat=my_post_grasp_retreat, id_grasp="random_grasp_" + str(grasp_num) ) list_grasps.append(grasp) return list_grasps
def talker(): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'map' g_pcloud = pc2.create_cloud_xyz32(header, wmap) g_pub_map.publish(g_pcloud) rospy.spin()
def HeaderGenerater(self): self.seq += 1 header = Header() header.seq = self.seq header.stamp = rospy.Time.now() header.frame_id = self.GoalFrame return header
def publish_data(self): h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id h.seq = self.seq self.seq = self.seq + 1 self.imu_msg.header = h q = self.device.read_quaternion() self.imu_msg.orientation.x = q[0] self.imu_msg.orientation.y = q[1] self.imu_msg.orientation.z = q[2] self.imu_msg.orientation.w = q[3] g = self.device.read_gyroscope() # convert from deg/sec to rad/sec self.imu_msg.angular_velocity.x = g[0] * math.pi / 180.0 self.imu_msg.angular_velocity.y = g[1] * math.pi / 180.0 self.imu_msg.angular_velocity.z = g[2] * math.pi / 180.0 a = self.device.read_linear_acceleration() self.imu_msg.linear_acceleration.x = a[0] self.imu_msg.linear_acceleration.y = a[1] self.imu_msg.linear_acceleration.z = a[2] self.imu_pub.publish(self.imu_msg) self.temp_msg.header = h self.temp_msg.temperature = self.device.read_temp() self.temp_pub.publish(self.temp_msg)
def main(): rospy.init_node("pose2d_to_odometry") own_num = rospy.get_param('~own_num', 0) global odom, head head = Header() head.stamp = rospy.Time.now() head.frame_id = "robot_{}/odom_combined".format(own_num) odom = Odometry() odom.child_frame_id = "robot_{}/base_footprint".format(own_num) o = 10**-9 # zero odom.pose.covariance = [1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, o, 0, 0, 0, # z doesn't change 0, 0, 0, o, 0, 0, # constant roll 0, 0, 0, 0, o, 0, # constant pitch 0, 0, 0, 0, 0, o] # guess - .3 radians rotation cov odom.twist.covariance = [100, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, # large covariances - no data 0, 0, 0, 100, 0, 0, 0, 0, 0, 0, 100, 0, 0, 0, 0, 0, 0, 100] global odom_pub odom_topic = rospy.get_param('~odometry_publish_topic', 'vo') pose2D_topic = rospy.get_param('~pose2D_topic', 'pose2D') odom_pub = rospy.Publisher(odom_topic, Odometry) rospy.Subscriber(pose2D_topic, Pose2D, on_pose) rospy.spin()
def fvToMsg(fv, now): global seq fvMsg = PersonFVMsg() header = Header() header.seq = seq header.stamp = now header.frame_id = frameID rhv = Vector3() rev = Vector3() lhv = Vector3() lev = Vector3() setV3(rhv, fv, 0) setV3(lhv, fv, 3) setV3(rev, fv, 6) setV3(lev, fv, 9) fvMsg.header = header fvMsg.rh = rhv fvMsg.re = rev fvMsg.lh = lhv fvMsg.le = lev fvMsg.distance = fv[12] return fvMsg
def create_move_group_joints_goal(joint_names, joint_values, group="right_arm_torso", plan_only=True): """ Creates a move_group goal based on pose. @arg joint_names list of strings of the joint names @arg joint_values list of digits with the joint values @arg group string representing the move_group group to use @arg plan_only bool to for only planning or planning and executing @return MoveGroupGoal with the desired contents""" header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() for name, value in zip(joint_names, joint_values): joint_c = JointConstraint() joint_c.joint_name = name joint_c.position = value joint_c.tolerance_above = 0.01 joint_c.tolerance_below = 0.01 joint_c.weight = 1.0 goal_c.joint_constraints.append(joint_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 1 moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True moveit_goal.request.group_name = group return moveit_goal
def test_4_remote(self): ''' Tests remote kill by publishing hearbeat, stopping and checking alarm is raised, then publishing hearbeat again to ensure alarm gets cleared. ''' # publishing msg to network pub = rospy.Publisher('/network', Header, queue_size=10) rate = rospy.Rate(10) t_end = rospy.Time.now() + rospy.Duration(1) while rospy.Time.now() < t_end: h = Header() h.stamp = rospy.Time.now() pub.publish(h) rate.sleep() self.reset_update() rospy.sleep(8.5) # Wait slighly longer then the timeout on killboard self.assert_raised() self.reset_update() t_end = rospy.Time.now() + rospy.Duration(1) while rospy.Time.now() < t_end: h = Header() h.stamp = rospy.Time.now() pub.publish(h) rate.sleep() self.AlarmBroadcaster.clear_alarm() self.assert_cleared()
def poseStampedToLabeledSphereMarker(cls, psdata, label, fmt="{label} %Y-%m-%d %H:%M:%S", zoffset=0.05): "[poseStamped, meta] -> sphereMarker" ps, meta = psdata res = [] h = Header() h.stamp = rospy.Time.now() h.frame_id = ps.header.frame_id sphere = Marker(type=Marker.SPHERE, action=Marker.ADD, header=h, id=cls.marker_id) sphere.scale.x = 0.07 sphere.scale.y = 0.07 sphere.scale.z = 0.07 sphere.pose = ps.pose sphere.color = ColorRGBA(1.0,0,0,1.0) sphere.ns = "db_play" sphere.lifetime = rospy.Time(3) cls.marker_id += 1 res.append(sphere) text = Marker(type=Marker.TEXT_VIEW_FACING, action=Marker.ADD, header=h, id=cls.marker_id) text.scale.z = 0.1 text.pose = deepcopy(ps.pose) text.pose.position.z += zoffset text.color = ColorRGBA(1.0,1.0,1.0,1.0) text.text = meta["inserted_at"].strftime(fmt).format(label=label) text.ns = "db_play_text" text.lifetime = rospy.Time(300) cls.marker_id += 1 res.append(text) return res
def make_header(frame_id, stamp=None): if stamp == None: stamp = rospy.Time.now() header = Header() header.stamp = stamp header.frame_id = frame_id return header
def get_ros_header(self): header = Header() header.stamp = rospy.Time.now() header.seq = self.sequence # http://www.ros.org/wiki/geometry/CoordinateFrameConventions#Multi_Robot_Support header.frame_id = self.frame_id return header
def computeICPBetweenScans(no1,no2, init_x = 0 , init_y = 0, init_yaw = 0): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'ibeo' example = cython_catkin_example.PyCCExample() # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no1)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no1)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no1)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no1)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no1)+'.txt') if opts.use_accumulated: test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no1)+'.txt')) if opts.icp_use_filtered_data: test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no1)+'.txt')) else: test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no1)+'.txt')) H_points = [[p[0], p[1],1] for p in test_cloud] # H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud] pc_point_t1 = pc2.create_cloud_xyz32(header, H_points) # print 'text1: ',len(test_cloud) # print test_cloud # example.load_2d_array('ref_map',np.array(test_cloud, np.float32)) # test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no2)+'.txt') # test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_filtered_'+str(no2)+'.txt') # test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_direct_'+str(no2)+'.txt') # test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_lm_filtered_'+str(no2)+'.txt') # test_cloud2 = read2DPointsFromTextFile('/home/avavav/avdata/alphard/onenorth/20150821-114839_sss/scan_filtered_'+str(no2)+'.txt') if opts.use_accumulated: test_cloud = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_lm_filtered_'+str(no2)+'.txt')) if opts.icp_use_filtered_data: test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_filtered_'+str(no2)+'.txt')) else: test_cloud2 = read2DPointsFromTextFile(os.path.join(dir_prefix,'scan_'+str(no2)+'.txt')) H_points = [[p[0], p[1],1] for p in test_cloud2] # H_points = [[np.float32(p[0]), np.float32(p[1]),1] for p in test_cloud] pc_point_t2 = pc2.create_cloud_xyz32(header, H_points) # print 'text2: ',len(test_cloud) # example.load_2d_array('que_map',np.array(test_cloud, np.float32)) # names = example.get_point_xyz_clouds_names() # print("Current point clouds: " + ",".join(names)) # icp_ros_result = g_processICP_srv(pc_point_t1, pc_point_t2) # print icp_ros_result # cython_icp_result = example.processICP(np.array(test_cloud, np.float32),np.array(test_cloud2, np.float32)) cython_icp_result = example.processICP(np.array([x[0] for x in test_cloud], np.float32),np.array([x[1] for x in test_cloud], np.float32),np.array([x[0] for x in test_cloud2], np.float32),np.array([x[1] for x in test_cloud2], np.float32), init_x, init_y, init_yaw) # print cython_icp_result return cython_icp_result
def retreat_move(self, height, velocity, forced=False): if forced: self.is_forced_retreat = True cart_pos, cart_quat = self.current_ee_pose(self.ee_frame, '/ellipse_frame') ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat)) #print "Retreat EP:", ell_pos latitude = ell_pos[0] if self.trim_retreat: latitude = min(latitude, TRIM_RETREAT_LATITUDE) #rospy.loginfo("[face_adls_manager] Retreating from current location.") retreat_pos = [latitude, ell_pos[1], height] retreat_pos = self.ellipsoid.enforce_bounds(retreat_pos) retreat_quat = [0,0,0,1] if forced: cart_path = self.ellipsoid.ellipsoidal_to_pose((retreat_pos, retreat_quat)) cart_msg = [PoseConv.to_pose_msg(cart_path)] else: ell_path = self.ellipsoid.create_ellipsoidal_path(ell_pos, ell_quat, retreat_pos, retreat_quat, velocity=0.01, min_jerk=True) cart_path = [self.ellipsoid.ellipsoidal_to_pose(ell_pose) for ell_pose in ell_path] cart_msg = [PoseConv.to_pose_msg(pose) for pose in cart_path] head = Header() head.frame_id = '/ellipse_frame' head.stamp = rospy.Time.now() pose_array = PoseArray(head, cart_msg) self.pose_traj_pub.publish(pose_array) self.is_forced_retreat = False
def test_AG_remote(self): self.assertEqual(self.AlarmListener.is_cleared(), True, msg='current state of kill signal is raised') # publishing msg to network pub = rospy.Publisher('/network', Header, queue_size=10) rate = rospy.Rate(10) t_end = rospy.Time.now() + rospy.Duration(3) while rospy.Time.now() < t_end: hello_header = Header() hello_header.stamp = rospy.Time.now() rospy.loginfo(hello_header) pub.publish(hello_header) rate.sleep() self.assertEqual( self.AlarmListener.is_cleared(), True, msg='REMOTE shutdown not worked. State = {}'.format( self._current_alarm_state)) # stop sending the msg, the remote alarm will raise when stop recieving # the msg for 8 secs rospy.sleep(8.2) self.assertEqual( self.AlarmListener.is_raised(), True, msg='REMOTE raised not worked. State = {}'.format( self._current_alarm_state))
def Publish(self): imu_data = self._hal_proxy.GetImu() imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self._frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.orientation.w = imu_data['orientation']['w'] imu_msg.orientation.x = imu_data['orientation']['x'] imu_msg.orientation.y = imu_data['orientation']['y'] imu_msg.orientation.z = imu_data['orientation']['z'] imu_msg.linear_acceleration.x = imu_data['linear_accel']['x'] imu_msg.linear_acceleration.y = imu_data['linear_accel']['y'] imu_msg.linear_acceleration.z = imu_data['linear_accel']['z'] imu_msg.angular_velocity.x = imu_data['angular_velocity']['x'] imu_msg.angular_velocity.y = imu_data['angular_velocity']['y'] imu_msg.angular_velocity.z = imu_data['angular_velocity']['z'] # Read the x, y, z and heading self._publisher.publish(imu_msg)
def execute(self, userdata): header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() userdata.move_it_joint_goal = MoveGroupGoal() goal_c = Constraints() for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose): joint_c = JointConstraint() joint_c.joint_name = name joint_c.position = value joint_c.tolerance_above = 0.01 joint_c.tolerance_below = 0.01 joint_c.weight = 1.0 goal_c.joint_constraints.append(joint_c) userdata.move_it_joint_goal.request.goal_constraints.append(goal_c) userdata.move_it_joint_goal.request.num_planning_attempts = 5 userdata.move_it_joint_goal.request.allowed_planning_time = 5.0 userdata.move_it_joint_goal.planning_options.plan_only = False # False = Plan + Execute ; True = Plan only userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group rospy.loginfo("Group Name: " + userdata.manip_joint_group) rospy.loginfo("Joints name: " + str(userdata.manip_joint_names)) rospy.loginfo("Joints Values: " + str(userdata.manip_goal_joint_pose)) rospy.loginfo("GOAL TO SEND IS:... " + str(userdata.move_it_joint_goal)) return "succeeded"
def publish_poses_grasps(grasps): rospy.loginfo("Publishing PoseArray on /grasp_pose_from_block_bla for grasp_pose") grasp_publisher = rospy.Publisher("grasp_pose_from_block_bla", PoseArray) graspmsg = Grasp() grasp_PA = PoseArray() header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() grasp_PA.header = header for graspmsg in grasps: print graspmsg print type(graspmsg) p = Pose(graspmsg.grasp_pose.pose.position, graspmsg.grasp_pose.pose.orientation) grasp_PA.poses.append(p) grasp_publisher.publish(grasp_PA) rospy.loginfo("Press a to continue...") while True: choice = raw_input("> ") if choice == 'a' : print "Continuing, a pressed" break else: grasp_publisher.publish(grasp_PA) rospy.sleep(0.1)
def cbClusters(self, ros_data): #Wait for filename to be received (if receiving from rostopic) if self.filename is None: return if self.run is False: self.pubMessages() return #Initialize object feature values if self.initialized is False: print "Reading object features from " + self.filename self.loadObjects() self.initialized = True #Read cluster message clusterArr = ros_data clusters = ros_data.objects self.transforms = ros_data.transforms #Classify clusters #self.labeled, self.tracked, self.errors, self.ids = self.run_filter(self.initX, self.labels, clusters) self.tracked, self.ids, self.error = self.getMatchingLabels(self.initX, self.labels, clusters) #Publish labels if len(clusters) is 0 or self.tracked is None: return self.outMsg = LabeledObjects() msgTime = rospy.Time.now() head = Header() head.stamp = msgTime self.outMsg.header = head self.outMsg.objects = self.tracked self.outMsg.labels = self.ids self.pubMessages()
def get_transform(tf_listener, frame1, frame2): temp_header = Header() temp_header.frame_id = frame1 temp_header.stamp = rospy.Time(0) try: tf_listener.waitForTransform(frame1, frame2, rospy.Time(0), rospy.Duration(5)) except tf.Exception, e: rethrow_tf_exception(e, "tf transform was not there between %s and %s"%(frame1, frame2))
def app_frame_add_callback(self,msg): rospy.loginfo("Heard App button press %s", msg.data) controlMessage = Header() #command to add waypoint; controlMessage.seq = 1; controlMessage.frame_id = msg.data; self._command_pub.publish(controlMessage)
def toObjects(self, points): objects = Objects header = Header() header.stamp = rospy.Time.now() header.frame_id = self.base_frame objects.header = header objects.objects = toPoints(points) return objects
def publishScan(no): header = Header() header.stamp = rospy.Time.now() header.frame_id = 'ibeo' test_cloud = read2DPointsFromTextFile('/home/avavav/avdata/alphard/medialink/20150918-180619/scan_'+str(no)+'.txt') H_points = [[p[0], p[1],1] for p in test_cloud] pc_point_t = pc2.create_cloud_xyz32(header, H_points) g_pub_ros.publish(pc_point_t)
def make_header(frame_id, stamp=None): ''' Creates a Header object for stamped ROS objects ''' if stamp == None: stamp = rospy.Time.now() header = Header() header.stamp = stamp header.frame_id = frame_id return header
def _HandleReceivedLine(self, line): self._Counter = self._Counter + 1 self._SerialPublisher.publish(String(str(self._Counter) + ", in: " + line)) if(len(line) > 0): lineParts = line.split('\t') try: if(lineParts[0] == 'quat'): self._qx = float(lineParts[1]) self._qy = float(lineParts[2]) self._qz = float(lineParts[3]) self._qw = float(lineParts[4]) if(lineParts[0] == 'ypr'): self._ax = float(lineParts[1]) self._ay = float(lineParts[2]) self._az = float(lineParts[3]) if(lineParts[0] == 'areal'): self._lx = float(lineParts[1]) self._ly = float(lineParts[2]) self._lz = float(lineParts[3]) imu_msg = Imu() h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id imu_msg.header = h imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 imu_msg.orientation.x = self._qx imu_msg.orientation.y = self._qy imu_msg.orientation.z = self._qz imu_msg.orientation.w = self._qw imu_msg.angular_velocity.x = self._ax imu_msg.angular_velocity.y = self._ay imu_msg.angular_velocity.z = self._az imu_msg.linear_acceleration.x = self._lx imu_msg.linear_acceleration.y = self._ly imu_msg.linear_acceleration.z = self._lz self.imu_pub.publish(imu_msg) except: rospy.logwarn("Error in Sensor values") rospy.logwarn(lineParts) pass
def __init__(self,tf_listener=None,arms=['R']): #rospy.loginfo('Greg version!') self.arms = arms self.tf_listener = tf_listener if self.tf_listener is None: self.tf_listener = tfx.TransformListener.instance() rospy.loginfo('waiting for transform') for arm in arms: self.tf_listener.waitForTransform('/0_link','/tool_'+arm,rospy.Time(0),rospy.Duration(5)) # ADDED, initializes the rest self.reset() self.pub_cmd = rospy.Publisher('raven_command', RavenCommand) self.state_sub = rospy.Subscriber('raven_state',raven_2_msgs.msg.RavenState,self._state_callback) self.header = Header() self.header.frame_id = '/0_link' self.timeout = Util.Timeout(4) self.stageLock = threading.Lock() self.thread = None self.isThreadPlaying = False ################# # PAUSE COMMAND # ################# header = Header() header.frame_id = MyConstants.Frames.Link0 header.stamp = rospy.Time.now() # create the tool pose toolCmd = ToolCommand() toolCmd.pose = tfx.pose([0,0,0]).msg.Pose() toolCmd.pose_option = ToolCommand.POSE_RELATIVE toolCmd.grasp_option = ToolCommand.GRASP_OFF # create the arm command armCmd = ArmCommand() armCmd.tool_command = toolCmd armCmd.active = True # create the raven command ravenPauseCmd = RavenCommand() ravenPauseCmd.arm_names.append(self.arms) ravenPauseCmd.arms.append(armCmd) ravenPauseCmd.pedal_down = True ravenPauseCmd.header = header ravenPauseCmd.controller = Constants.CONTROLLER_CARTESIAN_SPACE self.ravenPauseCmd = ravenPauseCmd
def callback(data): error = 0 warn_string = "" global prev_theta global prev_theta2 global prev_theta4 rate = rospy.Rate(f) r14 = float(data.pose.position.x) r24 = float(data.pose.position.y) r34 = float(data.pose.position.z) theta1 = atan(r24 / r14) alpha = r14 * cos(theta1) + r24 * sin(theta1) if (-a1**4 + 2 * a1**2 * a2**2 + 2 * a1**2 * alpha**2 + 2 * a1**2 * r34**2 - a2**4 + 2 * a2**2 * alpha**2 + 2 * a2**2 * r34**2 - alpha**4 - 2 * alpha**2 * r34**2 - r34**4) < 0: theta2 = prev_theta2 error = 2 else: theta2 = -2 * atan( (2 * a1 * r34 + (-a1**4 + 2 * a1**2 * a2**2 + 2 * a1**2 * alpha**2 + 2 * a1**2 * r34**2 - a2**4 + 2 * a2**2 * alpha**2 + 2 * a2**2 * r34**2 - alpha**4 - 2 * alpha**2 * r34**2 - r34**4)**(0.5)) / (a1**2 + 2 * a1 * alpha - a2**2 + alpha**2 + r34**2)) prev_theta2 = theta2 + pi / 2 theta2 = theta2 + pi / 2 if (-a1**2 + 2 * a1 * a2 - a2**2 + alpha**2 + r34**2) * (a1**2 + 2 * a1 * a2 + a2**2 - alpha**2 - r34**2) < 0: theta4 = prev_theta4 error = 2 else: theta4 = -2 * atan( (2 * a2 * r34 - ((-a1**2 + 2 * a1 * a2 - a2**2 + alpha**2 + r34**2) * (a1**2 + 2 * a1 * a2 + a2**2 - alpha**2 - r34**2))**(0.5)) / (-a1**2 + a2**2 + 2 * a2 * alpha + alpha**2 + r34**2)) prev_theta4 = theta4 if error == 2: warn_string = "Pozycja koncowki niemozliwa do osiagniecia \n" theta3 = theta4 - theta2 theta_vec = [theta1, theta2, theta3] i = 0 for joint in restrictions: if restrictions[i]['backward'] > theta_vec[i]: theta_vec[i] = restrictions[i]['backward'] warn_string = warn_string + "Przekroczono ograniczenie dolne stawu o numerze: " + str( i + 1) + "\n" error = 1 #rospy.logerr(warn_string) elif restrictions[i]['forward'] < theta_vec[i]: theta_vec[i] = restrictions[i]['forward'] warn_string = warn_string + "Przekroczono ograniczenie gorne stawu o numerze: " + str( i + 1) + "\n" #rospy.logerr(warn_string) error = 1 i = i + 1 if error == 1: theta_vec = prev_theta rospy.logerr(warn_string) elif error == 2: rospy.logerr(warn_string) else: prev_theta = theta_vec # create a new JoinState newJS = JointState() newJS.header = Header() newJS.header.stamp = rospy.Time.now() newJS.header.frame_id = 'base_link' newJS.name = ['base_to_link1', 'link1_to_link2', 'link2_to_link3'] #wskazanie jointow newJS.position = theta_vec pub.publish(newJS) rate.sleep()
def moveRobot(self): poruka = FollowJointTrajectoryActionGoal() h = Header() h.stamp = rospy.get_rostime() print 'Start H' print h print 'end H' poruka.header = h poruka.goal.trajectory.joint_names.append('arm_1_joint') poruka.goal.trajectory.joint_names.append('arm_2_joint') poruka.goal.trajectory.joint_names.append('arm_3_joint') poruka.goal.trajectory.joint_names.append('arm_4_joint') poruka.goal.trajectory.joint_names.append('arm_5_joint') poruka.goal.trajectory.joint_names.append('arm_6_joint') poruka.goal_id.id = 'Move trajectory ' + str(h.stamp.nsecs) h.stamp = rospy.get_rostime() poruka.goal.trajectory.header.stamp = h.stamp poruka.header.stamp = h.stamp tocka = JointTrajectoryPoint() ''' tocka.positions.append(0) tocka.positions.append(-1.58) # INVERTIRATI NA CANOPEN tocka.positions.append(1.38005) # -1.38005 INVERTIRATI na IPI!!!! tocka.positions.append(0) tocka.positions.append(1.36365) # -1.36365 # INVERTIRATI NA CANOPEN tocka.positions.append(0) ''' if (1 == 1): tocka.positions.append(0) tocka.positions.append(0) tocka.positions.append(0 * -1.57) # INVERTIRATI!!!! tocka.positions.append(0) tocka.positions.append(0) tocka.positions.append(0) else: tocka.positions.append(0) tocka.positions.append(0.0) tocka.positions.append(1.77) # INVERTIRATI!!!! tocka.positions.append(0.0) tocka.positions.append(-0.2) tocka.positions.append(0.9) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.time_from_start.nsecs = 0 * pow(10, 9) tocka.time_from_start.secs = 7 poruka.goal.trajectory.points.append(tocka) #self.trajPub_red.publish(poruka) self.trajPub_blue.publish(poruka) print "" print poruka print rospy.get_rostime() print h.stamp
def robot_scan_received(self, data): # wait until initialization is complete if not (self.initialized): return # we need to be able to transfrom the laser frame to the base frame if not (self.tf_listener.canTransform( self.base_frame, data.header.frame_id, data.header.stamp)): return # wait for a little bit for the transform to become avaliable (in case the scan arrives # a little bit before the odom to base_footprint transform was updated) self.tf_listener.waitForTransform(self.base_frame, self.odom_frame, data.header.stamp, rospy.Duration(0.5)) if not (self.tf_listener.canTransform( self.base_frame, data.header.frame_id, data.header.stamp)): return # calculate the pose of the laser distance sensor p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=data.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # determine where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=data.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # we need to be able to compare the current odom pose to the prior odom pose # if there isn't a prior odom pose, set the odom_pose variable to the current pose if not self.odom_pose_last_motion_update: self.odom_pose_last_motion_update = self.odom_pose return if self.particle_cloud: # check to see if we've moved far enough to perform an update curr_x = self.odom_pose.pose.position.x old_x = self.odom_pose_last_motion_update.pose.position.x curr_y = self.odom_pose.pose.position.y old_y = self.odom_pose_last_motion_update.pose.position.y curr_yaw = get_yaw_from_pose(self.odom_pose.pose) old_yaw = get_yaw_from_pose(self.odom_pose_last_motion_update.pose) if (np.abs(curr_x - old_x) > self.lin_mvmt_threshold or np.abs(curr_y - old_y) > self.lin_mvmt_threshold or np.abs(curr_yaw - old_yaw) > self.ang_mvmt_threshold): # This is where the main logic of the particle filter is carried out self.update_particles_with_motion_model() self.update_particle_weights_with_measurement_model(data) self.normalize_particles() self.resample_particles() self.update_estimated_robot_pose() self.publish_particle_cloud() self.publish_estimated_robot_pose() self.odom_pose_last_motion_update = self.odom_pose
def no_ball_poseStamped(self): return PoseStamped( header = Header(stamp = rospy.Time.now(), frame_id = "odom"), \ pose = Pose(position = Point(x = -1, y = -1, z = -1), orientation = Quaternion(w=1)))
def pcl_callback(self, pcl): """Callback for the pcl. Input should be a PointCloud2.""" self.rate_counter += 1 if self.rate_counter % 3 == 0: print 'Received Pointcloud. ' + str(time.time()) points_as_list = pc2_to_list(pcl) # Convert to List thresholded = self.thresholding(points_as_list) # Apply Thresholds # Uniform reduction uniform_reduction_factor = 1 thresholded = list( thresholded[i] for i in xrange(0, len(thresholded), uniform_reduction_factor)) # DEBUG Test publish thresholded points fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1) ] header = Header() header.frame_id = "map" cloud = pc2.create_cloud(header, fields, thresholded) cloud.header.stamp = rospy.Time.now() self.threshold_publisher.publish(cloud) # END Test publish if self.started: if self.pcl_reference == None: self.pcl_reference = o3d.geometry.PointCloud() # pylint: disable=no-member self.pcl_reference.points = o3d.utility.Vector3dVector( thresholded) # pylint: disable=no-member self.pcl_reference.estimate_normals( fast_normal_computation=False) # Store reference point position pcd_tree = o3d.geometry.KDTreeFlann(self.pcl_reference) # pylint: disable=no-member ref_point = None ref_point_nb = 0 # print len(self.pcl_reference.points) for i in range(0, len(self.pcl_reference.points)): nb = pcd_tree.search_radius_vector_3d( self.pcl_reference.points[i], 0.01) if nb > ref_point_nb: ref_point = self.pcl_reference.points[i] ref_point_nb = nb # print i self.position_reference = np.asarray( [[1.0, 0, 0, ref_point[0]], [0, 1.0, 0, ref_point[1]], [0, 0, 1.0, ref_point[2]], [0.0, 0.0, 0.0, 1.0]]) # print(self.position_reference) position_shaped = self.position_reference.reshape((16)) self.position_publisher.publish(position_shaped.tolist()) # self.started = False else: calctime = time.time() # Calculate diff from ICP head_tf = self.calc_head_transform_icp(thresholded) #print np.round(head_tf, decimals=4) # Calculate position # if not self.head_pos == None: # DEBUG PRINT # print (np.dot(self.position_reference, np.linalg.inv(head_tf)) - self.head_pos) #self.head_pos = np.dot(self.position_reference, np.linalg.inv(head_tf)) self.head_pos = np.dot(self.position_reference, head_tf) #print 'Calculation finished ' + str((time.time() - calctime)) print np.round(self.head_pos, decimals=4) #print "Reference:" #print np.round(self.position_reference, decimals=4) head_tf_shaped = self.head_pos.reshape((16)) # Reshape self.position_publisher.publish( head_tf_shaped.tolist()) # Publish # Eval self.calculation_times.append((time.time() - calctime)) self.positions.append(self.head_pos) self.ur5_pos_a.append(self.last_ur5_pos)
def image_callback(self, img: Image): rospy.loginfo_once("Recieved Message") t_start = time.time() if not self.camera.ready() or not self.trajectory_complete: return pts = [] self.camera.reset_position(publish_basecamera=True, timestamp=img.header.stamp) image = CvBridge().imgmsg_to_cv2(img, desired_encoding="rgb8") hsv = cv2.cvtColor(src=image, code=cv2.COLOR_BGR2HSV) h = self.camera.calculateHorizonCoverArea() cv2.rectangle(image, [0, 0], [640, h], [0, 0, 0], cv2.FILLED) # Field line detection mask2 = cv2.inRange(hsv, (0, 0, 255 - 65), (255, 65, 255)) out = cv2.bitwise_and(image, image, mask=mask2) kernel = np.ones((7, 7), np.uint8) out = cv2.morphologyEx(out, cv2.MORPH_CLOSE, kernel) cdst = cv2.cvtColor(out, cv2.COLOR_BGR2GRAY) retval, dst = cv2.threshold(cdst, 127, 255, cv2.THRESH_BINARY) edges = cv2.Canny(dst, 50, 150) lines = cv2.HoughLinesP(edges, rho=DetectorFieldline.HOUGH_RHO, theta=DetectorFieldline.HOUGH_THETA, threshold=DetectorFieldline.HOUGH_THRESHOLD, minLineLength=DetectorFieldline.HOUGH_MIN_LINE_LENGTH, maxLineGap=DetectorFieldline.HOUGH_MAX_LINE_GAP) ccdst = cv2.cvtColor(cdst, cv2.COLOR_GRAY2RGB) if lines is None: return for l in lines: x1, y1, x2, y2 = l[0] # computing magnitude and angle of the line if x2 == x1: angle = 0 else: angle = np.rad2deg(np.arctan((y2 - y1) / (x2 - x1))) pt1 = (x1, y1) pt2 = (x2, y2) if abs(angle) < 10: # Horizontal cv2.line(ccdst, pt1, pt2, (255, 0, 0), thickness=3, lineType=cv2.LINE_AA) elif abs(abs(angle) - 90) < 10: # Vertical cv2.line(ccdst, pt1, pt2, (0, 255, 0), thickness=3, lineType=cv2.LINE_AA) else: cv2.line(ccdst, pt1, pt2, (0, 0, 255), thickness=3, lineType=cv2.LINE_AA) if (pt2[0] - pt1[0]) == 0: continue slope = (pt2[1] - pt1[1]) / (pt2[0] - pt1[0]) b = y1 - slope * x1 for i in range(x1, x2, 15): y = slope * i + b pt = [i, y] pts.append(pt) if self.image_publisher.get_num_connections() > 0: img_out = CvBridge().cv2_to_imgmsg(ccdst) img_out.header = img.header self.image_publisher.publish(img_out) if self.point_cloud_publisher.get_num_connections() > 0: points3d = [] for p in pts: points3d.append(self.camera.findFloorCoordinate(p)) # Publish fieldlines in laserscan format header = Header() header.stamp = img.header.stamp header.frame_id = self.robot_name + "/base_camera" point_cloud_msg = pcl2.create_cloud_xyz32(header, points3d) if self.point_cloud_publisher.get_num_connections() > 0: self.point_cloud_publisher.publish(point_cloud_msg) t_end = time.time() rospy.loginfo_throttle(20, "Fieldline detection rate: " + str(t_end - t_start))
#! /usr/bin/env python import rospy from nav_msgs.msg import Odometry from std_msgs.msg import Header from gazebo_msgs.srv import GetModelState, GetModelStateRequest rospy.init_node('odom_pub') odom_pub = rospy.Publisher('/my_odom', Odometry) rospy.wait_for_service('/gazebo/get_model_state') get_model_srv = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) odom = Odometry() header = Header() header.frame_id = '/odom' model = GetModelStateRequest() model.model_name = 'simple_box' r = rospy.Rate(2) while not rospy.is_shutdown(): result = get_model_srv(model) odom.pose.pose = result.pose odom.twist.twist = result.twist header.stamp = rospy.Time.now() odom.header = header
def extract_and_publish_contours(self): if OPENCV_VERSION == 2: contours, hierarchy = cv2.findContours(self.threshed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) elif OPENCV_VERSION == 3: image, contours, hierarchy = cv2.findContours(self.threshed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) # http://docs.opencv.org/trunk/doc/py_tutorials/py_imgproc/py_contours/py_contour_features/py_contour_features.html try: header = Header(stamp=self.framestamp, frame_id=str(self.framenumber)) except: header = Header(stamp=None, frame_id=str(self.framenumber)) print 'could not get framestamp, run tracker_nobuffer instead' contour_info = [] for contour in contours: # Large objects are approximated by an ellipse if len(contour) > 5: x, y, ecc, area, angle = fit_ellipse_to_contour(self, contour) # if object is too large, split it in two, this helps with colliding objects, but is not 100% correct if area > self.params['max_expected_area']: slope = np.tan(angle) intercept = y - slope * x c1 = [] c2 = [] for point in contour: point = point.reshape(2) if is_point_below_line(point, slope, intercept): c1.append([point]) else: c2.append([point]) c1 = np.array(c1) c2 = np.array(c2) if len(c1) > 5: x, y, ecc, area, angle = fit_ellipse_to_contour( self, np.array(c1)) if area < self.params['max_size'] and area > self.params[ 'min_size']: data = add_data_to_contour_info( x, y, ecc, area, angle, self.dtCamera, header) contour_info.append(data) if len(c2) > 5: x, y, ecc, area, angle = fit_ellipse_to_contour( self, np.array(c2)) if area < self.params['max_size'] and area > self.params[ 'min_size']: data = add_data_to_contour_info( x, y, ecc, area, angle, self.dtCamera, header) contour_info.append(data) else: if area < self.params['max_size'] and area > self.params[ 'min_size']: data = add_data_to_contour_info(x, y, ecc, area, angle, self.dtCamera, header) contour_info.append(data) # Small ones just get a point else: area = 0 # publish the contours self.pubContours.publish(Contourlist(header=header, contours=contour_info)) return
from math import pi, radians from gazebo_msgs.msg import ModelState, ModelStates from nav_msgs.msg import Odometry, Path from tf import TransformBroadcaster import rospy import copy path_pub = rospy.Publisher('/global_path', Path, latch=True, queue_size=50) puntos = [[-1, 0], [-3.5, 0], [-3.5, 3.5], [1.5, 3.5], [1.5, -1.5], [3.5, -1.5], [3.5, -8.0], [-2.5, -8.0], [-2.5, -5.5], [1.5, -5.5], [1.5, -3.5], [-1, -3.5]] # puntos=[[-3.5,3.5],[1.5,3.5],[1.5,-1.5],[3.5,-1.5],[3.5,-8.0],[-2.5,-8.0],[-2.5,-5.5],[1.5,-5.5],[1.5,-3.5],[-1,-3.5]] # puntos=[[-3.5,0],[-3.5,3.5]] rospy.init_node('robot_path', anonymous=True) rate = rospy.Rate(1) # 10hz h = Header() path = Path() pose_t = PoseStamped() if __name__ == '__main__': try: print("dibujando el path en rviz") h.frame_id = "odom" path.header = h pose_t.header = h while not rospy.is_shutdown(): # h = Header() # path=Path() # pose_t=PoseStamped() # h.frame_id="odom" # path.header=h # pose_t.header=h
def test_vehicle_status(self): rospy.init_node('test_node', anonymous=True) msg = rospy.wait_for_message("/carla/ego_vehicle/vehicle_status", CarlaEgoVehicleStatus, timeout=15) self.assertNotEqual(msg.header, Header()) #todo: check frame-id self.assertNotEqual(msg.orientation, Quaternion())
def get_pose_stamped(self): """Will return a pose stamped object of the pose.""" pose = self.get_pose() header = Header(stamp=rospy.Time.now(), frame_id='base') return PoseStamped(header=header, pose=pose)
def right_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm right_arm = self.right_arm # 获取当前末端执行器位置姿态 pose_goal = right_arm.get_current_pose().pose # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55 : right_joint_const.position = 0.024 else: right_joint_const.position = 0 right_joint_const.weight = 1.0 # 限制1轴转动 right_joint1_const = JointConstraint() right_joint1_const.joint_name = "yumi_joint_1_r" right_joint1_const.position = 0 right_joint1_const.tolerance_above = 120 right_joint1_const.tolerance_below = 0 right_joint1_const.weight = 1.0 # 限制2轴转动 right_joint2_const = JointConstraint() right_joint2_const.joint_name = "yumi_joint_2_r" right_joint2_const.position = 0 right_joint2_const.tolerance_above = 0 right_joint2_const.tolerance_below = 150 right_joint2_const.weight = 1.0 # 限制3轴转动 right_joint3_const = JointConstraint() right_joint3_const.joint_name = "yumi_joint_3_r" right_joint3_const.position = 0 right_joint3_const.tolerance_above = 35 right_joint3_const.tolerance_below = 55 right_joint3_const.weight = 1.0 # 限制4轴转动 right_joint4_const = JointConstraint() right_joint4_const.joint_name = "yumi_joint_4_r" right_joint4_const.position = 0 right_joint4_const.tolerance_above = 60 right_joint4_const.tolerance_below = 75 right_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_r" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 right_joint6_const = JointConstraint() right_joint6_const.joint_name = "yumi_joint_6_r" right_joint6_const.position = 0 right_joint6_const.tolerance_above = 10 right_joint6_const.tolerance_below = 35 right_joint6_const.weight = 1.0 # 限制7轴转动 right_joint7_const = JointConstraint() right_joint7_const.joint_name = "yumi_joint_7_r" right_joint7_const.position = -10 right_joint7_const.tolerance_above = 0 right_joint7_const.tolerance_below = 160 right_joint7_const.weight = 1.0 # 限制末端位移 right_position_const = PositionConstraint() right_position_const.header = Header() right_position_const.link_name = "gripper_r_joint_r" right_position_const.target_point_offset.x = 0.5 right_position_const.target_point_offset.y = -0.5 right_position_const.target_point_offset.z = 1.0 right_position_const.weight = 1.0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = pose_goal.orientation right_orientation_const.link_name = "gripper_r_joint_r" right_orientation_const.absolute_x_axis_tolerance = 0.50 right_orientation_const.absolute_y_axis_tolerance = 0.25 right_orientation_const.absolute_z_axis_tolerance = 0.50 right_orientation_const.weight = 100 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const] consts.orientation_constraints = [right_orientation_const] # consts.position_constraints = [right_position_const] right_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Right_Qux pose_goal.orientation.y = Right_Quy pose_goal.orientation.z = Right_Quz pose_goal.orientation.w = Right_Quw pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053 pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12 pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47 right_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = 0.1644 # pose_goal.orientation.y = 0.3111 # pose_goal.orientation.z = 0.9086 # pose_goal.orientation.w = 0.2247 # pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053 # pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12 # pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47 # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = right_arm.plan() right_arm.execute(traj, wait=False) # 动作完成后清除目标信息 right_arm.clear_pose_targets() # 清除路径约束 right_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 right_arm.stop()
def left_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm left_arm = self.left_arm # 获取当前末端执行器位置姿态 pose_goal = left_arm.get_current_pose().pose # 限制末端夹具运动 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32 : left_joint_const.position = 0.024 else: left_joint_const.position = 0 left_joint_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = -0.5 left_position_const.target_point_offset.z = 1.0 left_position_const.weight = 1.0 # # 限制1轴转动 left_joint1_const = JointConstraint() left_joint1_const.joint_name = "yumi_joint_1_l" left_joint1_const.position = 0 left_joint1_const.tolerance_above = 1.76 left_joint1_const.tolerance_below = 0 left_position_const.weight = 1.0 # 限制2轴转动 left_joint2_const = JointConstraint() left_joint2_const.joint_name = "yumi_joint_2_l" left_joint2_const.position = 0 left_joint2_const.tolerance_above = 0 left_joint2_const.tolerance_below = 150 left_joint2_const.weight = 1.0 # 限制3轴转动 left_joint3_const = JointConstraint() left_joint3_const.joint_name = "yumi_joint_3_l" left_joint3_const.position = 0 left_joint3_const.tolerance_above = 35 left_joint3_const.tolerance_below = 55 left_joint3_const.weight = 1.0 # 限制4轴转动 left_joint4_const = JointConstraint() left_joint4_const.joint_name = "yumi_joint_4_l" left_joint4_const.position = 0 left_joint4_const.tolerance_above = 60 left_joint4_const.tolerance_below = 75 left_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_l" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 left_joint6_const = JointConstraint() left_joint6_const.joint_name = "yumi_joint_6_l" left_joint6_const.position = 0 left_joint6_const.tolerance_above = 10 left_joint6_const.tolerance_below = 35 left_joint6_const.weight = 1.0 # 限制7轴转动 left_joint7_const = JointConstraint() left_joint7_const.joint_name = "yumi_joint_7_l" left_joint7_const.position = -10 left_joint7_const.tolerance_above = 0 left_joint7_const.tolerance_below = 160 left_joint7_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = 0.25 left_position_const.target_point_offset.z = 0.5 left_position_const.weight = 1.0 # 添加末端姿态约束: left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = pose_goal.orientation left_orientation_const.link_name = "gripper_l_joint_r" left_orientation_const.absolute_x_axis_tolerance = 0.5 left_orientation_const.absolute_y_axis_tolerance = 0.25 left_orientation_const.absolute_z_axis_tolerance = 0.5 left_orientation_const.weight = 100 # 施加全约束 consts = Constraints() consts.joint_constraints = [left_joint_const] consts.orientation_constraints = [left_orientation_const] # consts.position_constraints = [left_position_const] left_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Left_Qux pose_goal.orientation.y = Left_Quy pose_goal.orientation.z = Left_Quz pose_goal.orientation.w = Left_Quw pose_goal.position.x = (Neurondata[11]-0.05)*1.48+0.053 pose_goal.position.y = (Neurondata[9]-0.18)*1.48+0.12 pose_goal.position.z = (Neurondata[10]-0.53)*1.48+0.47 left_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # left_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = left_arm.plan() left_arm.execute(traj, wait=False) # 动作完成后清除目标信息 left_arm.clear_pose_targets() # 清除路径约束 left_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 left_arm.stop()
def _plot(self, publisher, model, data, previous_ids=()): """Create markers for each object of the model and publish it as MarkerArray (also delete unused previously created markers)""" from rospy import get_rostime from std_msgs.msg import Header, ColorRGBA from geometry_msgs.msg import Point from visualization_msgs.msg import MarkerArray, Marker self.seq += 1 header = Header(frame_id='map', seq=self.seq, stamp=get_rostime()) # Common header for every marker marker_array = MarkerArray() for obj in model.geometryObjects: obj_id = model.getGeometryId(obj.name) # Prepare marker marker = Marker() marker.id = obj_id marker.header = header marker.action = Marker.ADD # same as Marker.MODIFY marker.pose = SE3ToROSPose(data.oMg[obj_id]) marker.color = ColorRGBA(*obj.meshColor) if obj.meshTexturePath != "": warnings.warn( "Textures are not supported in RVizVisualizer (for " + obj.name + ")", category=UserWarning, stacklevel=2) # Create geometry geom = obj.geometry if WITH_HPP_FCL_BINDINGS and isinstance(geom, hppfcl.ShapeBase): # append a primitive geometry if isinstance(geom, hppfcl.Cylinder): d, l = 2 * geom.radius, 2 * geom.halfLength marker.type = Marker.CYLINDER marker.scale = Point(d, d, l) marker_array.markers.append(marker) elif isinstance(geom, hppfcl.Box): size = npToTuple(2. * geom.halfSide) marker.type = Marker.CUBE marker.scale = Point(*size) marker_array.markers.append(marker) elif isinstance(geom, hppfcl.Sphere): d = 2 * geom.radius marker.type = Marker.SPHERE marker.scale = Point(d, d, d) marker_array.markers.append(marker) elif isinstance(geom, hppfcl.Capsule): d, l = 2 * geom.radius, 2 * geom.halfLength marker_array.markers.extend( create_capsule_markers(marker, data.oMg[obj_id], d, l)) else: msg = "Unsupported geometry type for %s (%s)" % ( obj.name, type(geom)) warnings.warn(msg, category=UserWarning, stacklevel=2) continue else: # append a mesh marker.type = Marker.MESH_RESOURCE # Custom mesh marker.scale = Point(*npToTuple(obj.meshScale)) marker.mesh_resource = 'file://' + obj.meshPath marker_array.markers.append(marker) # Remove unused markers new_ids = [marker.id for marker in marker_array.markers] for old_id in previous_ids: if not old_id in new_ids: marker_remove = Marker() marker_remove.header = header marker_remove.id = old_id marker_remove.action = Marker.DELETE marker_array.markers.append(marker_remove) # Publish markers publisher.publish(marker_array) # Return list of markers id return new_ids
def incredibly_basic(self): # If there is no background image, grab one, and move on to the next frame if self.backgroundImage is None: reset_background(self) return if self.reset_background_flag: reset_background(self) self.reset_background_flag = False return self.absdiff = cv2.absdiff(np.float32(self.imgScaled), self.backgroundImage) self.imgproc = copy.copy(self.imgScaled) retval, self.threshed = cv2.threshold(self.absdiff, self.params['threshold'], 255, 0) # convert to gray if necessary if len(self.threshed.shape) == 3: self.threshed = np.uint8( cv2.cvtColor(self.threshed, cv2.COLOR_BGR2GRAY)) # extract and publish contours # http://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_contours/py_contour_features/py_contour_features.html if OPENCV_VERSION == 2: contours, hierarchy = cv2.findContours(self.threshed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) elif OPENCV_VERSION == 3: self.threshed = np.uint8(self.threshed) image, contours, hierarchy = cv2.findContours(self.threshed, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) try: header = Header(stamp=self.framestamp, frame_id=str(self.framenumber)) except: header = Header(stamp=None, frame_id=str(self.framenumber)) print 'could not get framestamp, run tracker_nobuffer instead' contour_info = [] for contour in contours: if len(contour) > 5: # Large objects are approximated by an ellipse ellipse = cv2.fitEllipse(contour) (x, y), (a, b), angle = ellipse a /= 2. b /= 2. ecc = np.min((a, b)) / np.max((a, b)) area = np.pi * a * b data = Contourinfo() data.header = header data.dt = dtCamera data.x = x data.y = y data.area = area data.angle = angle data.ecc = ecc contour_info.append(data) else: # Small ones get ignored pass # publish the contours self.pubContours.publish(Contourlist(header=header, contours=contour_info))
def ik_test(limb, dx, dy, dz): ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() hdr = Header(stamp=rospy.Time.now(), frame_id='base') left_limb=baxter_interface.Limb('left') rospy.sleep(0.1) right_limb=baxter_interface.Limb('right') rospy.sleep(0.1) # left_limb.move_to_neutral() # right_limb.move_to_neutral() left_end=left_limb.endpoint_pose() left_pos=left_end['position'] left_or=left_end['orientation'] # right_end=right_limb.endpoint_pose() # right_pos=right_end['position'] # right_or=right_end['orientation'] xn=left_pos.x yn=left_pos.y zn=left_pos.z poses = { 'left': PoseStamped( header=hdr, pose=Pose( position=Point( x=xn+dx, y=yn+dy, z=zn+dz, ), orientation=Quaternion( x=left_or.x, y=left_or.y, z=left_or.z, w=left_or.w, ), ), ), 'right': PoseStamped( header=hdr, pose=Pose( position=Point( x=0.656982770038, y=-0.852598021641, z=0.0388609422173, ), orientation=Quaternion( x=0.367048116303, y=0.885911751787, z=-0.108908281936, w=0.261868353356, ), ), ), } ikreq.pose_stamp.append(poses[limb]) try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except (rospy.ServiceException, rospy.ROSException), e: rospy.logerr("Service call failed: %s" % (e,)) return 1
def convert_numpy_2_pointcloud2_color(points, stamp=None, frame_id=None, maxDistColor=None): # Clipping input. dist = np.linalg.norm(points, axis=1) if (maxDistColor is not None and maxDistColor > 0): dist = np.clip(dist, 0, maxDistColor) # Compose color. cr, cg, cb = color_map(dist, DIST_COLORS, DIST_COLOR_LEVELS) C = np.zeros((cr.size, 4), dtype=np.uint8) + 255 C[:, 0] = cb.astype(np.uint8) C[:, 1] = cg.astype(np.uint8) C[:, 2] = cr.astype(np.uint8) C = C.view("uint32") # Structured array. pointsColor = np.zeros( (points.shape[0], 1), \ dtype={ "names": ( "x", "y", "z", "rgba" ), "formats": ( "f4", "f4", "f4", "u4" )} ) points = points.astype(np.float32) pointsColor["x"] = points[:, 0].reshape((-1, 1)) pointsColor["y"] = points[:, 1].reshape((-1, 1)) pointsColor["z"] = points[:, 2].reshape((-1, 1)) pointsColor["rgba"] = C header = Header() if stamp is None: header.stamp = rospy.Time().now() else: header.stamp = stamp if frame_id is None: header.frame_id = "/map" else: header.frame_id = frame_id msg = PointCloud2() msg.header = header if len(points.shape) == 3: msg.height = points.shape[1] msg.width = points.shape[0] else: msg.height = 1 msg.width = points.shape[0] msg.fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), PointField('rgb', 12, PointField.UINT32, 1), ] msg.is_bigendian = False msg.point_step = 16 msg.row_step = msg.point_step * points.shape[0] msg.is_dense = int(np.isfinite(points).all()) msg.data = pointsColor.tostring() return msg
import matplotlib.pyplot as plt from os.path import join import time import sys import rosbag import ros_numpy import struct from sensor_msgs.msg import PointCloud2, PointField import sensor_msgs.point_cloud2 as pc2 from std_msgs.msg import Header from readpcd import offset_range_img, image_rows_full, image_cols, save_grey_img, create_4dir, progressbar from readbag import get_topics_types from numpy2pcd import range2xyz header = Header() header.frame_id = "map" fields = [ PointField('x', 0, PointField.FLOAT32, 1), PointField('y', 4, PointField.FLOAT32, 1), PointField('z', 8, PointField.FLOAT32, 1), ] # from point_cloud2.py def create_cloud_xyz32 def save_bag(range_imgs): bag = rosbag.Bag('test.bag', 'w') bag_topic = '/object_scan/raw_cloud' for i in range(0, range_imgs.shape[0]): clouds = range2xyz(range_imgs[i])
def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0]
def spin_once(self): def quat_from_orient(orient): '''Build a quaternion from orientation data.''' try: w, x, y, z = orient['quaternion'] return (x, y, z, w) except KeyError: pass try: return quaternion_from_euler(pi*orient['roll']/180., pi*orient['pitch']/180, pi*orient['yaw']/180.) except KeyError: pass try: m = identity_matrix() m[:3,:3] = orient['matrix'] return quaternion_from_matrix(m) except KeyError: pass # get data data = self.mt.read_measurement() # common header h = Header() h.stamp = rospy.Time.now() h.frame_id = self.frame_id # get data (None if not present) temp = data.get('Temp') # float raw_data = data.get('RAW') imu_data = data.get('Calib') orient_data = data.get('Orient') velocity_data = data.get('Vel') position_data = data.get('Pos') rawgps_data = data.get('RAWGPS') status = data.get('Stat') # int # create messages and default values imu_msg = Imu() imu_msg.orientation_covariance = (-1., )*9 imu_msg.angular_velocity_covariance = (-1., )*9 imu_msg.linear_acceleration_covariance = (-1., )*9 pub_imu = False gps_msg = NavSatFix() xgps_msg = GPSFix() pub_gps = False vel_msg = TwistStamped() pub_vel = False mag_msg = Vector3Stamped() pub_mag = False temp_msg = Float32() pub_temp = False # fill information where it's due # start by raw information that can be overriden if raw_data: # TODO warn about data not calibrated pub_imu = True pub_vel = True pub_mag = True pub_temp = True # acceleration imu_msg.linear_acceleration.x = raw_data['accX'] imu_msg.linear_acceleration.y = raw_data['accY'] imu_msg.linear_acceleration.z = raw_data['accZ'] imu_msg.linear_acceleration_covariance = (0., )*9 # gyroscopes imu_msg.angular_velocity.x = raw_data['gyrX'] imu_msg.angular_velocity.y = raw_data['gyrY'] imu_msg.angular_velocity.z = raw_data['gyrZ'] imu_msg.angular_velocity_covariance = (0., )*9 vel_msg.twist.angular.x = raw_data['gyrX'] vel_msg.twist.angular.y = raw_data['gyrY'] vel_msg.twist.angular.z = raw_data['gyrZ'] # magnetometer mag_msg.vector.x = raw_data['magX'] mag_msg.vector.y = raw_data['magY'] mag_msg.vector.z = raw_data['magZ'] # temperature # 2-complement decoding and 1/256 resolution x = raw_data['temp'] if x&0x8000: temp_msg.data = (x - 1<<16)/256. else: temp_msg.data = x/256. if rawgps_data: if rawgps_data['bGPS']<self.old_bGPS: pub_gps = True # LLA xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7 xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3 # NED vel # TODO? # Accuracy # 2 is there to go from std_dev to 95% interval xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3 xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3 self.old_bGPS = rawgps_data['bGPS'] if temp is not None: pub_temp = True temp_msg.data = temp if imu_data: try: x = imu_data['gyrX'] y = imu_data['gyrY'] z = imu_data['gyrZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) imu_msg.angular_velocity.x = v[0] imu_msg.angular_velocity.y = v[1] imu_msg.angular_velocity.z = v[2] imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0., radians(0.025), 0., 0., 0., radians(0.025)) pub_imu = True vel_msg.twist.angular.x = v[0] vel_msg.twist.angular.y = v[1] vel_msg.twist.angular.z = v[2] pub_vel = True except KeyError: pass try: x = imu_data['accX'] y = imu_data['accY'] z = imu_data['accZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) imu_msg.linear_acceleration.x = v[0] imu_msg.linear_acceleration.y = v[1] imu_msg.linear_acceleration.z = v[2] imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0., 0.0004, 0., 0., 0., 0.0004) pub_imu = True except KeyError: pass try: x = imu_data['magX'] y = imu_data['magY'] z = imu_data['magZ'] v = numpy.array([x, y, z]) v = v.dot(self.R) mag_msg.vector.x = v[0] mag_msg.vector.y = v[1] mag_msg.vector.z = v[2] pub_mag = True except KeyError: pass if velocity_data: pub_vel = True vel_msg.twist.linear.x = velocity_data['Vel_X'] vel_msg.twist.linear.y = velocity_data['Vel_Y'] vel_msg.twist.linear.z = velocity_data['Vel_Z'] if orient_data: pub_imu = True orient_quat = quat_from_orient(orient_data) imu_msg.orientation.x = orient_quat[0] imu_msg.orientation.y = orient_quat[1] imu_msg.orientation.z = orient_quat[2] imu_msg.orientation.w = orient_quat[3] imu_msg.orientation_covariance = (radians(1.), 0., 0., 0., radians(1.), 0., 0., 0., radians(9.)) if position_data: pub_gps = True xgps_msg.latitude = gps_msg.latitude = position_data['Lat'] xgps_msg.longitude = gps_msg.longitude = position_data['Lon'] xgps_msg.altitude = gps_msg.altitude = position_data['Alt'] if status is not None: if status & 0b0001: self.stest_stat.level = DiagnosticStatus.OK self.stest_stat.message = "Ok" else: self.stest_stat.level = DiagnosticStatus.ERROR self.stest_stat.message = "Failed" if status & 0b0010: self.xkf_stat.level = DiagnosticStatus.OK self.xkf_stat.message = "Valid" else: self.xkf_stat.level = DiagnosticStatus.WARN self.xkf_stat.message = "Invalid" if status & 0b0100: self.gps_stat.level = DiagnosticStatus.OK self.gps_stat.message = "Ok" else: self.gps_stat.level = DiagnosticStatus.WARN self.gps_stat.message = "No fix" self.diag_msg.header = h self.diag_pub.publish(self.diag_msg) if pub_gps: if status & 0b0100: gps_msg.status.status = NavSatStatus.STATUS_FIX xgps_msg.status.status = GPSStatus.STATUS_FIX gps_msg.status.service = NavSatStatus.SERVICE_GPS xgps_msg.status.position_source = 0b01101001 xgps_msg.status.motion_source = 0b01101010 xgps_msg.status.orientation_source = 0b01101010 else: gps_msg.status.status = NavSatStatus.STATUS_NO_FIX xgps_msg.status.status = GPSStatus.STATUS_NO_FIX gps_msg.status.service = 0 xgps_msg.status.position_source = 0b01101000 xgps_msg.status.motion_source = 0b01101000 xgps_msg.status.orientation_source = 0b01101000 # publish available information if pub_imu: imu_msg.header = h self.imu_pub.publish(imu_msg) if pub_gps: xgps_msg.header = gps_msg.header = h self.gps_pub.publish(gps_msg) self.xgps_pub.publish(xgps_msg) if pub_vel: vel_msg.header = h self.vel_pub.publish(vel_msg) if pub_mag: mag_msg.header = h self.mag_pub.publish(mag_msg) if pub_temp: self.temp_pub.publish(temp_msg)
#!/usr/bin/python import rospy import moveit_python from moveit_msgs.msg import CollisionObject from geometry_msgs.msg import Pose, Point, Quaternion from shape_msgs.msg import SolidPrimitive from std_msgs.msg import Header rospy.init_node("moveit_table_scene") scene = moveit_python.PlanningSceneInterface("base_link") #pub = rospy.Publisher("/move_group/monitored_planning_scene", CollisionObject, queue_size=1) table = CollisionObject() # Header h = Header() h.stamp = rospy.Time.now() # Shape box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = [0.808, 1.616, 2.424] # Pose position = Point(*[1.44, 0.0, 0.03]) orient = Quaternion(*[-0.510232, 0.49503, 0.515101, 0.478832]) # Create Collision Object scene.addSolidPrimitive("table", box, Pose(*[position, orient]))
timestampFormat = "{:0>4d}-{:0>2d}-{:0>2d} {:0>2d}:{:0>2d}:{:0>2d}".format( years, mouth, day, hour, minutes, second) # print("timestamp format",timestampFormat) timeArray = time.strptime(timestampFormat, "%Y-%m-%d %H:%M:%S") timeStamp = int(time.mktime(timeArray)) # timeStamp = timeStamp*1000 + mils # print("timeStamp:",timeStamp) data_array = data[20:] image_size = image_height * image_width * 2 if len(data_array) != image_size: print("error size:", len(data_array)) continue # print("ok size:",len(data_array)) #publish temp data image_temp = Image() header = Header() header.stamp.secs = timeStamp header.stamp.nsecs = mils * 1000000 image_temp.header = header image_temp.height = image_height image_temp.width = image_width image_temp.encoding = 'rgb8' image_temp.data = str(data_array) temp_pub.publish(image_temp) else: no_response = no_response + 1 if no_response > 600: print(["no_response:", no_response]) no_response = 0 break rate.sleep()
def crane_solver(self, data): P1 = [0,0,0] P2 = [0,0,0] c = 1.484 # speed of sound in 20 C water per uSec #c = 0.343 #speed of sound in air hydro = rospy.ServiceProxy('hydrophones/hydrophone_position', Hydrophone_locations_service) holder = hydro() self.hydro0 = holder.hydro0_xyz self.hydro1 = holder.hydro1_xyz self.hydro2 = holder.hydro2_xyz self.hydro3 = holder.hydro3_xyz #position of hydrophones x1 = self.hydro1[0] #+ 0.75 #np.random.uniform(-1, 1) #in mm x2 = self.hydro2[0] #- 0.75 #np.random.uniform(-1, 1) y2 = self.hydro2[1] #+ 0.75 #np.random.uniform(-1, 1) x3 = self.hydro3[0] #- 0.75 #np.random.uniform(-1, 1) y3 = self.hydro3[1] #+ 0.75 #np.random.uniform(-1, 1) z3 = self.hydro3[2] #print "x1: %f" % x1 calc_service = rospy.ServiceProxy('hydrophones/calculated_time_stamps', Calculated_time_stamps_service) #calc_service = rospy.ServiceProxy('hydrophones/actual_time_stamps', Actual_time_stamps_service) tstampts = calc_service() '''calc_service = rospy.ServiceProxy('hydrophones/actual_time_stamps', Actual_time_stamps_service) tstampts = calc_service() del1 = (tstampts.actual_time_stamps[1])*c #mm/uSec del2 = (tstampts.actual_time_stamps[2])*c #mm/uSec del3 = (tstampts.actual_time_stamps[3])*c #mm/uSec ''' #print tstampts.calculated_time_stamps[1] #print tstampts.calculated_time_stamps[2] #print tstampts.calculated_time_stamps[3] #print tstampts.actual_time_stamps[1] #print tstampts.actual_time_stamps[2] #print tstampts.actual_time_stamps[3] del1 = (tstampts.calculated_time_stamps[1])*c #mm/uSec del2 = (tstampts.calculated_time_stamps[2])*c #mm/uSec del3 = (tstampts.calculated_time_stamps[3])*c #mm/uSec #del1 = (tstampts.actual_time_stamps[1])*c #mm/uSec #del2 = (tstampts.actual_time_stamps[2])*c #mm/uSec #del3 = (tstampts.actual_time_stamps[3])*c #mm/uSec '''#Experiment x1 = 8.500000 x2 = 5.650000 y2 = 4.750000 x3 = -1.750000 y3 = 3.500000 z3 = 1.750000 del1 = 2.053896 del2 = -1.349004 del3 = -2.817121 ''' #print del1 #print del2 #print del3 print "********" #print "\n" #print "del1: %f del2: %f del3: %f" % (del1, del2, del3) left = x1/del1 if del1 != 0 else 0 right = x2/del2 if del2 != 0 else 0 #print "left = %f B1 = %f right = %f" % (left, B1, right) A1 = left - right # eqn (12) B1 = -y2/del2 if del2 != 0 else 0 #print "A1: %f" % A1 catch = (x1*x1-del1*del1)/(2.0*del1) if del1 != 0 else 0 zero = (x2*x2 + y2*y2-del2*del2)/(2.0*del2) if del2 != 0 else 0 D1 = zero - catch #print "D1: %f" % D1 Q1 = -z3/del3 if del3 != 0 else 0 Q2aleft = (A1*y3)/(B1*del3) if (B1*del3) !=0 else 0 Q2amiddle = x3/del3 if del3 != 0 else 0 Q2aright = x1/del1 if del1 != 0 else 0 Q2a = Q2aleft - Q2amiddle + Q2aright Q2bleft = (del1*del1-x1*x1)/(2.0*del1) if (2.0*del1) != 0 else 0 Q2bmiddle = (D1*y3)/(B1*del3) if (B1*del3) != 0 else 0 Q2bright = (del3*del3 - x3*x3 - y3*y3 - z3*z3)/(2*del3) if (2*del3) != 0 else 0 Q2b = Q2bleft + Q2bmiddle - Q2bright R1aleft = (A1*A1)/(B1*B1) if (B1*B1) != 0 else 0 R1aright = (x1*x1)/(del1*del1) if (del1*del1) != 0 else 0 R1a = R1aleft + 1 - R1aright R1bleft = (x1*x1*x1)/(del1*del1) if (del1*del1) != 0 else 0 R1bright = (2.0*A1*D1)/(B1*B1) if (B1*B1) != 0 else 0 R1b = R1bleft - x1 + R1bright R1cleft = (D1*D1)/(B1*B1) if (B1*B1) != 0 else 0 R1cright = (x1*x1*x1*x1)/(4.0*del1*del1) if (4.0*del1*del1) != 0 else 0 R1c = (x1*x1)/2 - (del1*del1)/4 + R1cleft - R1cright AA = Q1*Q1*R1a + Q2a*Q2a BB = Q1*Q1*R1b + 2.0*Q2a*Q2b CC = Q1*Q1*R1c + Q2b*Q2b discr = BB*BB - 4.0*AA*CC ; if (discr < 0): #no real solution was found; set garbage values for P1 and P2 and return 0 P1[0] = P1[1] = P1[2] = 0.0 P2[0] = P2[1] = P2[2] = 0.0 else: P1[0] = (-BB+math.sqrt(discr))/(2.0*AA) if (2.0*AA) != 0 else 0 P2[0] = (-BB-math.sqrt(discr))/(2.0*AA) if (2.0*AA) != 0 else 0 # get corresponding value for y coordinate ; eqn (11) P1[1] = -(A1*P1[0]+D1)/B1 if B1 != 0 else 0 P2[1] = -(A1*P2[0]+D1)/B1 if B1 != 0 else 0 # get correspoinding value for z coordinate ; eqn (16) P1[2] = -(Q2a*P1[0]+Q2b)/Q1 if Q1 != 0 else 0 P2[2] = -(Q2a*P2[0]+Q2b)/Q1 if Q1 != 0 else 0 #print "x: %f, y: %f, z: %f" % (x,y,z) P1sum = abs(P1[0]) + abs(P1[1]) + abs(P1[2]) P2sum = abs(P2[0]) + abs(P2[1]) + abs(P2[2]) if P1sum > P2sum: x = P1[0] y = P1[1] z = P1[2] rospy.loginfo("x1: %f" % (P1[0])) rospy.loginfo("y1: %f" % (P1[1])) rospy.loginfo("z1: %f" % (P1[2])) else: x = P2[0] y = P2[1] z = P2[2] rospy.loginfo("x2: %f" % (P2[0])) rospy.loginfo("y2: %f" % (P2[1])) rospy.loginfo("z2: %f" % (P2[2])) self.crane_pub = rospy.Publisher('hydrophones/crane_pos', Crane_pos, queue_size = 1) self.crane_pub.publish(Crane_pos( header=Header(stamp=rospy.Time.now(), frame_id='Crane_pos_calc'), x_pos=x, y_pos=y, z_pos=z)) return Crane_pos_serviceResponse(x, y, z)
def scan_received(self, msg): """ This is the default logic for what to do when processing scan data. Feel free to modify this, however, I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """ if not (self.initialized): # wait for initialization to complete return if not (self.tf_listener.canTransform( self.base_frame, msg.header.frame_id, msg.header.stamp)): # need to know how to transform the laser to the base frame # this will be given by either Gazebo or neato_node return if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, msg.header.stamp)): # need to know how to transform between base and odometric frames # this will eventually be published by either Gazebo or neato_node return # calculate pose of laser relative ot the robot base p = PoseStamped( header=Header(stamp=rospy.Time(0), frame_id=msg.header.frame_id)) self.laser_pose = self.tf_listener.transformPose(self.base_frame, p) # find out where the robot thinks it is based on its odometry p = PoseStamped(header=Header(stamp=msg.header.stamp, frame_id=self.base_frame), pose=Pose()) self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p) # store the the odometry pose in a more convenient format (x,y,theta) new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose) if not (self.particle_cloud): # now that we have all of the necessary transforms we can update the particle cloud self.initialize_particle_cloud() # cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh self.current_odom_xy_theta = new_odom_xy_theta # update our map to odom transform now that the particles are initialized self.fix_map_to_odom_transform(msg) elif (len(self.current_odom_xy_theta) < 3 or math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh): # we have moved far enough to do an update! self.update_particles_with_odom(msg) # update based on odometry if self.last_projected_stable_scan: last_projected_scan_timeshift = deepcopy( self.last_projected_stable_scan) last_projected_scan_timeshift.header.stamp = msg.header.stamp self.scan_in_base_link = self.tf_listener.transformPointCloud( "base_link", last_projected_scan_timeshift) self.update_particles_with_laser(msg) # update based on laser scan self.update_robot_pose() # update robot's pose self.resample_particles( ) # resample particles to focus on areas of high density self.fix_map_to_odom_transform( msg ) # update map to odom transform now that we have new particles # publish particles (so things like rviz can see them) self.publish_particles(msg)
positions.append(0.0) joints.append('head_j2') positions.append(0.0) joints.append('head_j3') positions.append(0.0) joints.append('head_j4') positions.append(0.0) joints.append('head_j5') positions.append(0.0) joints.append('head_j6') positions.append(0.0) header = Header(0,rospy.Time.now(),'0') pub.publish(JointState(header, joints, positions, [0]*len(positions), [0]*len(positions))) try: while not rospy.is_shutdown(): time.sleep(1.0) header = Header(0,rospy.Time.now(),'0') pub.publish(JointState(header, joints, positions, [0]*len(positions), [0]*len(positions))) except (KeyboardInterrupt,EOFError,rospy.ROSInterruptException): pass
def Stamped(obj, stamp, frame_id): obj.header = Header(frame_id=frame_id, stamp=stamp) return obj
def _update_viz_core(self): """Updates visualization after a change.""" # Create a new IM control. menu_control = InteractiveMarkerControl() menu_control.interaction_mode = InteractiveMarkerControl.BUTTON menu_control.always_visible = True # Multiplex marker types added based on action step type. if self.action_step.type == ActionStep.ARM_TARGET: # Handle "normal" steps (saved poses). menu_control = self._make_gripper_marker(menu_control, self._is_hand_open()) elif self.action_step.type == ActionStep.ARM_TRAJECTORY: # Handle trajectories. # First, get all trajectory positions. point_list = [] for j in range(len(self.action_step.armTrajectory.timing)): point_list.append(self._get_traj_pose(j).position) # Add a main maker for all points in the trajectory (sphere # list). # NOTE(jstn): This will not work anymore, trajectories are # unsupported for now. menu_control.markers.append( Marker(type=Marker.SPHERE_LIST, id=self.get_uid(), lifetime=TRAJ_MARKER_LIFETIME, scale=SCALE_TRAJ_STEP_SPHERES, header=Header(frame_id=frame_id), color=COLOR_TRAJ_STEP_SPHERES, points=point_list)) # Add a marker for the first step in the trajectory. menu_control.markers.append( ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_FIRST, self._get_traj_pose(0), frame_id, TRAJ_ENDPOINT_SCALE)) # Add a marker for the last step in the trajectory. last_index = len(self.action_step.armTrajectory.timing) - 1 menu_control.markers.append( ActionStepMarker._make_sphere_marker( self.get_uid() + ID_OFFSET_TRAJ_LAST, self._get_traj_pose(last_index), frame_id, TRAJ_ENDPOINT_SCALE)) else: # Neither "normal" pose nor trajectory; error... rospy.logerr('Non-handled action step type ' + str(self.action_step.type)) # Add an arrow to the relative object, if there is one. base_pose = world.get_absolute_pose(self.get_target()) # base_pose refers to the wrist link, which looks weird in # visualizations, so offset_pose is shifted forward to be in the # "middle" of the gripper offset_pose = ActionStepMarker._offset_pose(base_pose, 1) ref_frame = world.get_ref_from_name(self._get_ref_name()) if ref_frame == ArmState.OBJECT: menu_control.markers.append( Marker(type=Marker.ARROW, id=(ID_OFFSET_REF_ARROW + self.get_uid()), scale=SCALE_OBJ_REF_ARROW, header=Header(frame_id=BASE_LINK), color=COLOR_OBJ_REF_ARROW, points=[ offset_pose.position, self.get_target().refFrameLandmark.pose.position ])) # Make and add the text for this step ('Step X'). text_pos = Point() text_pos.x = offset_pose.position.x text_pos.y = offset_pose.position.y text_pos.z = offset_pose.position.z + TEXT_Z_OFFSET menu_control.markers.append( Marker(type=Marker.TEXT_VIEW_FACING, id=self.get_uid(), scale=SCALE_STEP_TEXT, text='Step ' + str(self.step_number), color=COLOR_STEP_TEXT, header=Header(frame_id=BASE_LINK), pose=Pose(text_pos, Quaternion(0, 0, 0, 1)))) # Make and add interactive marker. int_marker = InteractiveMarker() int_marker.name = self._get_name() int_marker.header.frame_id = BASE_LINK int_marker.pose = offset_pose int_marker.scale = INT_MARKER_SCALE self._add_6dof_marker(int_marker, True) int_marker.controls.append(menu_control) ActionStepMarker._im_server.insert(int_marker, self.marker_feedback_cb)
def both_arms_go_to_pose_goal(self): # 设置动作对象变量,此处为both_arms both_arms = self.both_arms # 获取当前各轴转角 axis_angle = both_arms.get_current_joint_values() # print axis_angle # 获取当前末端执行器位置姿态 right_pose_goal = both_arms.get_current_pose(end_effector_link="gripper_r_finger_r") left_pose_goal = both_arms.get_current_pose(end_effector_link="gripper_l_finger_r") print right_pose_goal # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55 : right_joint_const.position = 0.024 else: right_joint_const.position = 0 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32 : left_joint_const.position = 0.024 else: left_joint_const.position = 0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = right_pose_goal.pose.orientation right_orientation_const.link_name = "gripper_r_joint_r" right_orientation_const.absolute_x_axis_tolerance = 0.6 right_orientation_const.absolute_y_axis_tolerance = 0.6 right_orientation_const.absolute_z_axis_tolerance = 0.6 right_orientation_const.weight = 1 left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = left_pose_goal.pose.orientation left_orientation_const.link_name = "gripper_l_joint_r" left_orientation_const.absolute_x_axis_tolerance = 0.6 left_orientation_const.absolute_y_axis_tolerance = 0.6 left_orientation_const.absolute_z_axis_tolerance = 0.6 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const, left_joint_const] # consts.orientation_constraints = [right_orientation_const, left_orientation_const] both_arms.set_path_constraints(consts) # # 设置动作对象目标位置姿态 # # 右臂 # right_pose_goal.pose.orientation.x = Right_Qux # right_pose_goal.pose.orientation.y = Right_Quy # right_pose_goal.pose.orientation.z = Right_Quz # right_pose_goal.pose.orientation.w = Right_Quw # right_pose_goal.pose.position.x = Neurondata[5] # right_pose_goal.pose.position.y = Neurondata[3] # right_pose_goal.pose.position.z = Neurondata[4] # # 左臂 # left_pose_goal.pose.orientation.x = Left_Qux # left_pose_goal.pose.orientation.y = Left_Quy # left_pose_goal.pose.orientation.z = Left_Quz # left_pose_goal.pose.orientation.w = Left_Quw # left_pose_goal.pose.position.x = Neurondata[11] # left_pose_goal.pose.position.y = Neurondata[9] # left_pose_goal.pose.position.z = Neurondata[10] # # 右臂 # right_pose_goal.pose.orientation.x = Right_Qux # right_pose_goal.pose.orientation.y = Right_Quy # right_pose_goal.pose.orientation.z = Right_Quz # right_pose_goal.pose.orientation.w = Right_Quw # right_pose_goal.pose.position.x = (1266/740)*(Neurondata[5]+0.28)-0.495 # right_pose_goal.pose.position.y = (1295/780)*(Neurondata[3]+0.56)-0.754 # right_pose_goal.pose.position.z = (1355/776)*(Neurondata[4]-0.054)-0.184 # # 左臂 # left_pose_goal.pose.orientation.x = Left_Qux # left_pose_goal.pose.orientation.y = Left_Quy # left_pose_goal.pose.orientation.z = Left_Quz # left_pose_goal.pose.orientation.w = Left_Quw # left_pose_goal.pose.position.x = (1266/850)*(Neurondata[11]+0.33)-0.495 # left_pose_goal.pose.position.y = (1295/720)*(Neurondata[9]+0.19)-0.541 # left_pose_goal.pose.position.z = (1355/745)*(Neurondata[10]-0.055)-0.184 # 右臂 right_pose_goal.pose.orientation.x = Right_Qux right_pose_goal.pose.orientation.y = Right_Quy right_pose_goal.pose.orientation.z = Right_Quz right_pose_goal.pose.orientation.w = Right_Quw right_pose_goal.pose.position.x = (Neurondata[5]-0.05)*1.48+0.053 right_pose_goal.pose.position.y = (Neurondata[3]+0.18)*1.48-0.12 right_pose_goal.pose.position.z = (Neurondata[4]-0.53)*1.48+0.47 # 左臂 left_pose_goal.pose.orientation.x = Left_Qux left_pose_goal.pose.orientation.y = Left_Quy left_pose_goal.pose.orientation.z = Left_Quz left_pose_goal.pose.orientation.w = Left_Quw left_pose_goal.pose.position.x = (Neurondata[11]-0.05)*1.48+0.053 left_pose_goal.pose.position.y = (Neurondata[9]-0.18)*1.48+0.12 left_pose_goal.pose.position.z = (Neurondata[10]-0.53)*1.48+0.47 # # 右臂 # right_pose_goal.pose.orientation.x = right_pose_goal.pose.orientation.x # right_pose_goal.pose.orientation.y = right_pose_goal.pose.orientation.y # right_pose_goal.pose.orientation.z = right_pose_goal.pose.orientation.z # right_pose_goal.pose.orientation.w = right_pose_goal.pose.orientation.w # right_pose_goal.pose.position.x = right_pose_goal.pose.position.x # right_pose_goal.pose.position.y = right_pose_goal.pose.position.y # right_pose_goal.pose.position.z = right_pose_goal.pose.position.z # # 左臂 # left_pose_goal.pose.orientation.x = left_pose_goal.pose.orientation.x # left_pose_goal.pose.orientation.y = left_pose_goal.pose.orientation.y # left_pose_goal.pose.orientation.z = left_pose_goal.pose.orientation.z # left_pose_goal.pose.orientation.w = left_pose_goal.pose.orientation.w # left_pose_goal.pose.position.x = left_pose_goal.pose.position.x # left_pose_goal.pose.position.y = left_pose_goal.pose.position.y # left_pose_goal.pose.position.z = left_pose_goal.pose.position.z # 设置动作组的两个目标点 both_arms.set_pose_target(right_pose_goal, end_effector_link="gripper_r_finger_r") both_arms.set_pose_target(left_pose_goal, end_effector_link="gripper_l_finger_r") # 规划和输出动作 traj = both_arms.plan() both_arms.execute(traj, wait=False) # # 清除路径约束 both_arms.clear_path_constraints() # 确保输出停止 both_arms.stop()
def create_point_cloud_message(self, pts): header = Header() header.stamp = rospy.Time.now() header.frame_id = '/world' cloud_message = pcl2.create_cloud_xyz32(header, pts) return cloud_message
def run(): global vx_ global vy_ global target_ global poses_ global createkeyboard_ global destroykeyboard_ global exist_ rx = 0 ry = 0 pressed = pygame.key.get_pressed() for event in pygame.event.get(): # determin if X was clicked, or Ctrl+W or Alt+F4 was used if event.type == pygame.QUIT: sys.exit(0) elif event.type == pygame.KEYDOWN: # switch human control if event.key == pygame.K_f: target_ += 1 if target_ == len(exist_): target_ = 0 elif event.key == pygame.K_j: target_ -= 1 if target_ == -1: target_ = len(exist_) - 1 # look control if event.key == pygame.K_e: exist_[target_] = not exist_[target_] # face direction control if event.key == pygame.K_w: vy_ = 0.05 ry += 1 pass if event.key == pygame.K_a: vx_ = -0.05 rx -= 1 pass if event.key == pygame.K_s: vy_ = -0.05 ry -= 1 pass if event.key == pygame.K_d: vx_ = 0.05 rx += 1 pass elif event.type == pygame.KEYUP: if event.key == pygame.K_w: vy_ = 0.0 pass if event.key == pygame.K_a: vx_ = 0.0 pass if event.key == pygame.K_s: vy_ = 0.0 pass if event.key == pygame.K_d: vx_ = 0.0 pass # speech menu control if createkeyboard_: createKeyboard() createkeyboard_ = False if destroykeyboard_: cv2.destroyWindow(__keyboard__) destroykeyboard_ = False # update poses_[target_].position.x += vx_ poses_[target_].position.y += vy_ if rx != 0 or ry != 0: if rx > 0: if ry == 0: rx = 1 ry = 0 else: if ry > 0: # w + d rx = 0.923879 ry = 0.382683 else: # s + d rx = -0.923879 ry = 0.382683 elif rx == 0: rx = 0.707107 if ry > 0: ry = 0.707107 else: ry = -0.707107 else: if ry == 0: rx = 0 ry = 1 else: if ry > 0: # w + a rx = 0.382683 ry = 0.923879 else: # s + a rx = 0.382683 ry = -0.923879 poses_[target_].orientation = Quaternion(rx, 0, 0, ry) for i, pose in enumerate(poses_): if exist_[i]: pub_detected_person.publish(header=Header( stamp=rospy.get_rostime(), frame_id='map'), id=str(i), position3d_map=pose.position, position3d_camera=Point(0, 0, 0), pose3d_camera=Quaternion(1, 0, 0, 0), map_to_camera=pose)
def _cloud_cb(self, cloud): points = np.array(list(read_points(cloud))) if points.shape[0] == 0: return pos = points[:,0:3] cor = np.reshape(points[:,-1], (points.shape[0], 1)) # Get 4x4 matrix which transforms point cloud co-ords to odometry frame try: points_to_map = self.tf.asMatrix('/lasths', cloud.header) except tf.ExtrapolationException: return transformed_points = points_to_map.dot(np.vstack((pos.T, np.ones((1, pos.shape[0]))))) transformed_points = transformed_points[:3,:].T self.seq += 1 header = Header() header.seq = self.seq header.stamp = rospy.Time.now() header.frame_id = '/lasths' self.cloud = np.vstack((self.cloud, np.hstack((transformed_points, cor)))) if self.seq % 30 == 0: print "plup!" self.cloud = np.zeros((0, 4)) output_cloud = create_cloud(header, cloud.fields, self.cloud) self.cloud_pub.publish(output_cloud)