def publish(self, rgb, temp_array): # Publish RGB image rgb_img_msg = self.bridge.cv2_to_imgmsg(rgb) rgb_img_msg.header.frame_id = self.evo_thermal_frame rgb_img_msg.header.stamp = rospy.Time.now() self.rgb_publisher.publish(rgb_img_msg) # Publish temperatures array raw_temp_array = Float64MultiArray() raw_temp_array.layout.dim.append(MultiArrayDimension()) raw_temp_array.layout.dim[0].size = 32 raw_temp_array.layout.dim[0].stride = 1 raw_temp_array.layout.dim[0].label = "x-axis" raw_temp_array.layout.dim.append(MultiArrayDimension()) raw_temp_array.layout.dim[1].size = 32 raw_temp_array.layout.dim[1].stride = 1 raw_temp_array.layout.dim[1].label = "y-axis" data = np.reshape(temp_array, (1024)) np.round(data, 2) raw_temp_array.data = data self.raw_publisher.publish(raw_temp_array) # Publish internal sensor temperature self.ptat_publisher.publish(self.ptat / self.scaling - self.celsius_offset)
def reconstruction(data): arduino_data = data.data # recosntruction #just to test dummy reconstruction #have to delete when the actual reconstruction is implemented arduino_data = np.array(arduino_data) arduino_data = arduino_data + 0.12 #reconstruction end here # send message mat_size = m * n reconstruction_data = Float32MultiArray() reconstruction_data.layout.dim.append(MultiArrayDimension()) reconstruction_data.layout.dim.append(MultiArrayDimension()) reconstruction_data.layout.dim[0].label = "W" reconstruction_data.layout.dim[1].label = "H" reconstruction_data.layout.dim[0].size = m reconstruction_data.layout.dim[1].size = n reconstruction_data.layout.dim[0].stride = m * n reconstruction_data.layout.dim[1].stride = m reconstruction_data.layout.data_offset = 0 reconstruction_data.data = arduino_data #[0]* mat_size print("--------Start of Reconstruction data------") rospy.loginfo(reconstruction_data) print("---------End of Reconstruction data ------") pub.publish(reconstruction_data)
def __init__(self): #Initialize arbotix comunications print"\nArbotix initialization, wait 10 seconds..." ArbotiX.__init__(self) for x in xrange(1,21): time.sleep(0.5) print(str(x*0.5) + "/10s") #Setupr velocities and positions vectors and messages self.joints_poses = [0,0,0,0,0,0] self.joints_vels = [0,0,0,0,0] self.ee_closed = 0 self.vels_to_pub = Float32MultiArray() self.poses_to_pub = Float32MultiArray() self.poses_layout = MultiArrayDimension('joints_poses', 6, 0) self.vels_layout = MultiArrayDimension('joints_vels', 5, 0) self.poses_to_pub.layout.dim = [self.poses_layout] self.poses_to_pub.layout.data_offset = 0 self.vels_to_pub.layout.dim = [self.vels_layout] self.vels_to_pub.layout.data_offset = 0 #ROS pubblisher for joint velocities and positions self.pos_pub = rospy.Publisher('/windowx_3links/joints_poses', Float32MultiArray, queue_size=1) self.vel_pub = rospy.Publisher('/windowx_3links/joints_vels', Float32MultiArray, queue_size=1) print"Windowx_3link node created, whaiting for messages in:" print" windowx_2links/torque" print"Publishing joints' positions and velocities in:" print" /windowx_3links/joints_poses" print" /windowx_3links/joints_vels" #Start publisher self.publish()
def im_callback(self, data): outputs, im = self.objsegmenter.segment(data, y_crop_idx=650) labels = self.objsegmenter.get_labels(outputs) box_list = np.asarray( self.objsegmenter.get_boxes(outputs).tensor.tolist()) if self.visualize: out = self.objsegmenter.draw_output(outputs, im) # Publish segmented image self.image_pub.publish(cv2_to_imgmsg(out.get_image(), 'bgr8')) # Publish segmentation data as Segmentation message seg_msg = Segmentation() # Add labels seg_msg.classes = labels # Flatten data into 1d array seg_msg.boxes.data = list(box_list.flatten()) # Populate layout info dims = box_list.shape if len(dims) > 1: seg_msg.boxes.layout.dim = [ MultiArrayDimension(), MultiArrayDimension() ] seg_msg.boxes.layout.dim[0].label = 'classes' seg_msg.boxes.layout.dim[0].size = dims[0] seg_msg.boxes.layout.dim[0].stride = dims[0] * dims[1] seg_msg.boxes.layout.dim[1].label = 'bbox coordinates' seg_msg.boxes.layout.dim[1].size = dims[1] seg_msg.boxes.layout.dim[1].stride = dims[1] print(seg_msg.classes) print(seg_msg.boxes.data) else: print('No objects found') self.segment_pub.publish(seg_msg)
def publish_bounding_boxes(self, bbs, image_width, image_height): # Given a list of bounding boxes, publish same. # The detector returns an array of dlib.rectangle's. if len(bbs) > 0: msg = Int32MultiArray() msg.layout.data_offset = 0 msg.layout.dim.append( MultiArrayDimension(label='height', size=len(bbs) + 1, stride=4 * (len(bbs) + 1))) msg.layout.dim.append( MultiArrayDimension(label='width', size=4, stride=1)) msg.layout.dim.append( MultiArrayDimension(label='channel', size=1, stride=1)) data = [] # The first entry is the size of the source image data.append(0) data.append(0) data.append(image_width) data.append(image_height) for rect in bbs: data.append(rect.left()) data.append(rect.top()) data.append(rect.right()) data.append(rect.bottom()) msg.data = data self.bounding_box_publisher.publish(msg) return
def numpyArray2MultiArray(na, ma): s = na.shape for i in np.arange(len(s)): d = MultiArrayDimension() d.size = s[i] ma.layout.dim.append(d) ma.data = np.reshape(na, -1)
def demo(): ''' Publish sample data to ROS ''' # Read demo binvox as (64*64*64) array with open('demo/occupy.binvox', 'rb') as f: occ = binvox_rw.read_as_3d_array(f).data with open('demo/non_occupy.binvox', 'rb') as f: non = binvox_rw.read_as_3d_array(f).data msg = ByteMultiArray() msg.data = (occ.astype(int) - non.astype(int)).flatten().tolist() msg.layout.dim.append( MultiArrayDimension(label='x', size=DIM, stride=DIM * DIM * DIM)) msg.layout.dim.append( MultiArrayDimension(label='y', size=DIM, stride=DIM * DIM)) msg.layout.dim.append(MultiArrayDimension(label='z', size=DIM, stride=DIM)) rospy.init_node('shape_demo_loader') pub = rospy.Publisher('local_occupancy', numpy_msg(ByteMultiArray), queue_size=10) rospy.Subscriber("local_occupancy_predicted", numpy_msg(ByteMultiArray), callback) time.sleep(5) pub.publish(msg) rospy.spin()
def vox_to_float_array(voxel_grid, dim): out_msg = Float32MultiArray() out_msg.data = voxel_grid.astype(np.float32).flatten().tolist() out_msg.layout.dim.append(MultiArrayDimension(label='x', size=dim, stride=dim*dim*dim)) out_msg.layout.dim.append(MultiArrayDimension(label='y', size=dim, stride=dim*dim)) out_msg.layout.dim.append(MultiArrayDimension(label='z', size=dim, stride=dim)) return out_msg
def determine_vision_pattern(self): resolution = self.occ_grid.info.resolution sensor_range = self.sensor.range_max dimension = 2 * int( np.round(math.ceil((sensor_range) / resolution - 0.5))) + 1 self.vision_pattern.data = [] self.vision_pattern.layout.dim.append(MultiArrayDimension()) self.vision_pattern.layout.dim.append(MultiArrayDimension()) self.vision_pattern.layout.dim[0].size = dimension self.vision_pattern.layout.dim[1].size = dimension self.vision_pattern.layout.dim[0].label = "rows" self.vision_pattern.layout.dim[1].label = "cols" self.vision_pattern.layout.data_offset = 0 centre_index = (dimension + 1) / 2 - 1 # -1 since index starts at 0 for i in range(0, dimension): for j in range(0, dimension): distance = resolution * np.sqrt((i - centre_index)**2 + (j - centre_index)**2) if distance <= sensor_range: self.vision_pattern.data.append(1) else: self.vision_pattern.data.append(0)
def __init__(self, rosTopic, minAmplitude, maxAmplitude, numDoFs, period): """ The constructor. Keyword arguments: rosTopic -- The ROS topic on which to publish the goal. minAmplitude -- The minimum amplitude in radians. maxAmplitude -- The maximum amplitude in radians. numDoFs -- The number of DoFs. period -- The period of the square wave in seconds. """ self.period = period self.rosTopic = rosTopic # Define the dimensions of the message dim = MultiArrayDimension() dim.size = numDoFs dim.label = "goalMsg" dim.stride = 1 # Define the goal message containing the upper bound of the square wave self.goalMsgHigh = Float64MultiArray() for ii in range(0, numDoFs): self.goalMsgHigh.data.append(maxAmplitude) self.goalMsgHigh.layout.dim.append(dim) self.goalMsgHigh.layout.data_offset = 0 # Define the goal message containing the lower bound of the square wave self.goalMsgLow = Float64MultiArray() for ii in range(0, numDoFs): self.goalMsgLow.data.append(minAmplitude) self.goalMsgLow.layout.dim.append(dim) self.goalMsgLow.layout.data_offset = 0
def publish(self): ''' ''' PathMsg = PathDisplay() PathMsg.xGlbPath = self.dataPlanning['xGlbPath'] PathMsg.yGlbPath = self.dataPlanning['yGlbPath'] # different from the usage in std_msgs dim = MultiArrayDimension() dim.stride = int(self.dataPlanning['indOptimal']) dim.size = self.dataPlanning['pathCandidate'].shape[0] PathMsg.xLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,0]).flatten() PathMsg.xLocPath.layout.dim = [dim] PathMsg.yLocPath.data = np.array(self.dataPlanning['pathCandidate'][:,1]).flatten() PathMsg.yLocPath.layout.dim = [dim] PathMsg.header.stamp = rospy.Time.now() basePathPub.publish(PathMsg) PlanMsg = PlanOP() PlanMsg.prePoint.x = self.prePoint[0] PlanMsg.prePoint.y = self.prePoint[1] PlanMsg.prePoint.z = 0. PlanMsg.xCtrPath = self.dataPlanning['xCtrPath'] PlanMsg.yCtrPath = self.dataPlanning['yCtrPath'] PlanMsg.distObst = self.dataPlanning['distObst'] PlanMsg.desireDirDrive = self.desireDirDrive PlanMsg.curDirDrive = self.curDirDrive PlanMsg.flagIsShifting = self.flagIsShifting PlanMsg.distYClose = self.distYClose PlanMsg.modeDrive = self.modeDrive PlanMsg.vbrMode = self.vbrMode PlanMsg.startedFlag = self.toCloudStartedFlag PlanMsg.endTaskFlag = self.toCloudEndTaskFlag PlanMsg.header.stamp = rospy.Time.now() planPub.publish(PlanMsg)
def moveit_callback(msg): pub = rospy.Publisher('/update_joint_position', Float64MultiArray) stock_pub = rospy.Publisher('/stock_joint_position', Float64MultiArray) global plan_only r = rospy.Rate(10) names = msg.result.planned_trajectory.joint_trajectory.joint_names points = msg.result.planned_trajectory.joint_trajectory.points for point in points: pos_msg = Float64MultiArray() i = 0 for name in names: dim = MultiArrayDimension() dim.label = name pos_msg.layout.dim.append(dim) pos_msg.data = point.positions if rospy.get_param('~sim_mode') and plan_only == False: pub.publish(pos_msg) elif plan_only: stock_pub.publish(pos_msg) r.sleep() if plan_only: global robot_trajectory robot_trajectory = msg.result.planned_trajectory
def callback(msg,prevMarkers): detectedMarkers = msg.markers now = rospy.get_time() #the current time in seconds for detectedMarker in detectedMarkers: measuredTime = detectedMarker.header.stamp.secs markerID = detectedMarker.id prevMarkers[markerID] = measuredTime detected_markers = [] for marker in prevMarkers.keys(): if abs(prevMarkers[marker]-now) > 5: #if the measurement has been stale for 5 seconds del prevMarkers[marker] else: detected_markers.append(marker) array1 = MultiArrayDimension() array1.label = 'numMarkers' array1.size = len(detected_markers) array1.size = len(detected_markers) layout = MultiArrayLayout() layout.dim = [array1,] layout.data_offset = 0 msg = Int16MultiArray() msg.layout = layout msg.data = detected_markers pub.publish(msg)
def __init__(self): # Define the dimensions of the message rightHandGoalDim = MultiArrayDimension() rightHandGoalDim.size = NUM_DOFS rightHandGoalDim.label = "rightHandGoal" rightHandGoalDim.stride = 1 # Define the goal messages self.rightHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.rightHandGoalMsg.data.append(0) self.rightHandGoalMsg.layout.dim.append(rightHandGoalDim) self.rightHandGoalMsg.layout.data_offset = 0 leftHandGoalDim = MultiArrayDimension() leftHandGoalDim.size = NUM_DOFS leftHandGoalDim.label = "leftHandGoal" leftHandGoalDim.stride = 1 self.leftHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.leftHandGoalMsg.data.append(0) self.leftHandGoalMsg.layout.dim.append(leftHandGoalDim) self.leftHandGoalMsg.layout.data_offset = 0
def test_strify_message(self): from genpy.message import Message, strify_message, fill_message_args def roundtrip(m): yaml_text = strify_message(m) print(yaml_text) loaded = yaml.load(yaml_text) print("loaded", loaded) new_inst = m.__class__() if loaded is not None: fill_message_args(new_inst, [loaded]) else: fill_message_args(new_inst, []) return new_inst # The following tests have not been ported to genpy yet # test array of Messages field. We can't use M4 or M5 because fill_message_args has to instantiate the embedded type from test_roslib_comm.msg import ArrayOfMsgs from std_msgs.msg import String, Time, MultiArrayLayout, MultiArrayDimension dims1 = [MultiArrayDimension(*args) for args in [('', 0, 0), ('x', 1, 2), ('y of z', 3, 4)]] dims2 = [MultiArrayDimension('hello world', 91280, 1983274)] times = [Time(genpy.Time(*args)) for args in [(0,), (12345, 6789), (1, 1)]] val = ArrayOfMsgs([String(''), String('foo'), String('bar of soap')], times, [MultiArrayLayout(dims1, 0), MultiArrayLayout(dims2, 12354)], ) self.assertEquals(val, roundtrip(val))
def test_dictionary_with_child_message(self): from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension expected_message = Float64MultiArray(layout=MultiArrayLayout( dim=[ MultiArrayDimension(label='Dimension1', size=12, stride=7), MultiArrayDimension(label='Dimension2', size=90, stride=8) ], data_offset=1), data=[1.1, 2.2, 3.3]) dictionary = { 'layout': { 'dim': [{ 'label': expected_message.layout.dim[0].label, 'size': expected_message.layout.dim[0].size, 'stride': expected_message.layout.dim[0].stride }, { 'label': expected_message.layout.dim[1].label, 'size': expected_message.layout.dim[1].size, 'stride': expected_message.layout.dim[1].stride }], 'data_offset': expected_message.layout.data_offset }, 'data': expected_message.data } message = message_converter.convert_dictionary_to_ros_message( 'std_msgs/Float64MultiArray', dictionary) expected_message = serialize_deserialize(expected_message) self.assertEqual(message, expected_message)
def get_example_cov(example_id=0): """ Creates an example covariance matrix. The data in the matrix was acquired from the maven-1.bag file. :param example_id: A constant id of the example in the list of stored examples. Minimum value is 0. Values above the max value will be set to 0 instead. Current maximum value is: 1 :return: The covariance matrix in format std_msgs/Float64MultiArray """ MAX_SAMPLES = 1 if example_id > MAX_SAMPLES: example_id = 0 nan = float("NaN") examples = [] # Here, just copy-paste some more data from maven-1.bag so that you can choose which example you want examples.append([0.2256878004660412, 0.004743161046803848, nan, nan, nan, 0.4799785723084568, 6.694665570936009e-05, 0.004743161046804125, 0.03333087258142175, nan, nan, nan, 0.0064891656361629295, 0.08962944598353237, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 0.47997857230845664, 0.006489165636162725, nan, nan, nan, 2.4285886983849885, -0.06696549234373295, 6.694665570941213e-05, 0.08962944598353195, nan, nan, nan, -0.06696549234373322, 1.424665892676366]) examples.append([0.5758368975539181, -0.10477581457466455, nan, nan, nan, 3.003595362397707, -0.39924730334819275, -0.10477581457466437, 0.24081097327513568, nan, nan, nan, -0.4539729065847597, 1.439606811146181, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, nan, 3.003595362397709, -0.45397290658476147, nan, nan, nan, 18.613535338951223, -1.9899796692884495, -0.39924730334819236, 1.4396068111461806, nan, nan, nan, -1.9899796692884442, 10.681110693507879]) # ----------------- cov_list = examples[example_id] # Create Float64MultiArray similar to those in the bag files dim_0 = MultiArrayDimension(label="", size=7, stride=49) dim_1 = MultiArrayDimension(label="", size=7, stride=7) cov_dim = [dim_0, dim_1] cov_layout = MultiArrayLayout(dim=cov_dim, data_offset=0) return Float64MultiArray(layout=cov_layout, data=cov_list)
def publish_depth(self, frames): depth_frame = frames.get_depth_frame() if self.DISPLAY: depth_image = np.asanyarray( self.colorizer.colorize(depth_frame).get_data()) cv2.imshow("Original Depth", depth_image) cv2.waitKey(2) depth = np.asanyarray(depth_frame.get_data(), dtype=np.float64) depth = self.crop_depth(depth) # Create depth Float64MultiArray message to publish depth_data = Float64MultiArray() dim1 = MultiArrayDimension(label="height", size=depth.shape[0]) dim2 = MultiArrayDimension(label="width", size=depth.shape[1]) depth_data.layout.dim.extend([dim1, dim2]) depth_data.data = depth.flatten() # Display Cropped Image if self.DANIEL_DEBUG and not self.DISPLAY: displayed_depth = np.asanyarray( self.colorizer.colorize(depth_frame).get_data()) cv2.imshow("Published Depth", self.crop_depth(displayed_depth)) cv2.waitKey(2) self.safe_publish(self.depth_pub, depth_data)
def main(): global pub rospy.init_node('go_to_point') pub = rospy.Publisher('/diff_drive_controller/cmd_vel', Twist, queue_size=1) status_pub = rospy.Publisher('/move_bot/status', Bool, queue_size=1) sub_odom = rospy.Subscriber('/odom',Odometry, clbk_odom) target_sub = rospy.Subscriber('/move_bot/goal',Pose, clbk_goal) mat = Float64MultiArray() mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim.append(MultiArrayDimension()) gripper_pub = rospy.Publisher('/gripper_controller/command', Float64MultiArray, queue_size=1) rate =rospy.Rate(200) extend = 0 gripper_extend = 0 global message_arrived while not rospy.is_shutdown(): if message_arrived: if state_ == 0: fix_yaw(desired_position_) elif state_ == 1: go_straight_ahead(desired_position_) elif state_ == 2: orient_yaw(yaw_required) elif state_ ==3: done() change_state(0) message_arrived = False status = True status_msg = Bool() status_msg.data = status status_pub.publish(status_msg) print("done")
def image_to_saliency(t, saliency, saliency_pub, saliency_image_pub, bridge, image, last_time, elapsed): if t < 1.0: return if image.value is None: return if last_time.value is None: last_time.value = t current_time = t dt = current_time - last_time.value last_time.value = current_time elapsed.value = elapsed.value + dt if elapsed.value < 0.01: return else: elapsed.value = 0. image = bridge.value.imgmsg_to_cv2(image.value, "bgr8") saliency_map = saliency.value.compute_saliency_map(image) saliency_map_image = bridge.value.cv2_to_imgmsg( np.uint8(saliency_map * 255.), "mono8") saliency_image_pub.value.publish(saliency_map_image) from std_msgs.msg import Float32MultiArray, MultiArrayDimension, MultiArrayLayout height = MultiArrayDimension(size=len(saliency_map)) width = MultiArrayDimension(size=len(saliency_map[0])) lo = MultiArrayLayout([height, width], 0) saliency_pub.value.publish( Float32MultiArray(layout=lo, data=saliency_map.flatten()))
def publish(self): #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10) pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1) rate = rospy.Rate(50) #10Hz a = [0.0, 0.0] dim = MultiArrayDimension() dim.size = len(a) dim.label = "REMOTEmsg" dim.stride = len(a) apub = Float64MultiArray() apub.data = a apub.layout.dim.append(dim) apub.layout.data_offset = 0 while not rospy.is_shutdown(): #UART_read = ser.readline() #UART_read = ser.Serial.readline() UART_read = self.dev.readline() steer_cmd = int(UART_read[0:4]) throt_cmd = int(UART_read[4:8]) a = [throt_cmd, steer_cmd] rospy.loginfo(a) apub.data = a pub.publish(apub) rate.sleep()
def handle_p2b(req): global theta print("here") theta = list(req.configs.angles) delta = 0.0001 f_original = computeH(0, 0, 0, 0) f_theta1 = computeH(delta, 0, 0, 0) f_theta2 = computeH(0, delta, 0, 0) f_theta3 = computeH(0, 0, delta, 0) f_theta4 = computeH(0, 0, 0, delta) J1 = (f_theta1 - f_original) / delta J2 = (f_theta2 - f_original) / delta J3 = (f_theta3 - f_original) / delta J4 = (f_theta4 - f_original) / delta jacobian = Float64MultiArray() jacobian.layout.dim = [ MultiArrayDimension('height', 4, 3 * 4), MultiArrayDimension('width', 3, 3) ] jacobian.data = [ J1[0], J2[0], J3[0], J4[0], J1[1], J2[1], J3[1], J4[1], J1[2], J2[2], J3[2], J4[2] ] return jacobian
def motor_joint_positions_publisher(): #Initiate node for controlling joint1 and joint2 positions. rospy.init_node('motor_positions_node', anonymous=True) #Define publishers for each joint position controller commands. pub = rospy.Publisher('bicopter_motor_controller/command', Float64MultiArray, queue_size=10) rate = rospy.Rate(100) #100 Hz #Defining msg as float64MultiArray msg = Float64MultiArray() msg.layout.data_offset = 0 myMultiArrayDimensions = MultiArrayDimension() myMultiArrayDimensions.label = '' myMultiArrayDimensions.size = 2 myMultiArrayDimensions.stride = 1 msg.layout.dim.append(myMultiArrayDimensions) #While loop to have joints follow a certain position, while rospy is not shutdown. i = 0 while not rospy.is_shutdown(): #Have each joint follow a sine movement of sin(i/100). msg.data = [0.00, 0.00] #Publish the same sine movement to each joint. pub.publish(msg) i = i + 1 #increment i rate.sleep() #sleep for rest of rospy.Rate(100)
def enableGravityCompMode(self): index = raw_input("Put robot into gravity compensation mode? Y/n\n") if index == "N" or index == "n": return True else: # Define the dimensions of the message dim = MultiArrayDimension() dim.size = NUM_DOFS dim.label = "goalMsg" dim.stride = 1 kpMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): kpMsg.data.append(0) # Kp gains for gravity compensation mode kpMsg.layout.dim.append(dim) kpMsg.layout.data_offset = 0 print "Setting posture task Kp gains to be zero..." self.postureTaskKpPublisher.publish(kpMsg) kdMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): kdMsg.data.append(5) # Kd gains for gravity compensation mode kdMsg.layout.dim.append(dim) kdMsg.layout.data_offset = 0 print "Setting posture task Kd gains to be zero..." self.postureTaskKdPublisher.publish(kdMsg) print "Done setting robot into gravity compensation mode." return not rospy.is_shutdown()
def __init__(self, name): self.name = name self.position_stddev = 0.50 # [m] self.ref_frame = "/map" rospy.init_node('kf_example') self.position_stddev = rospy.get_param("~position_stddev_m", self.position_stddev) rospy.loginfo("Starting KF Example") self.array = Float64MultiArray() self.array.layout = MultiArrayLayout() self.array.layout.data_offset = 0 self.array.layout.dim = [MultiArrayDimension('data', 3, 3)] self.array.data = [0.0] * 3 self.array2 = Float64MultiArray() self.array2.layout = MultiArrayLayout() self.array2.layout.data_offset = 0 self.array2.layout.dim = [MultiArrayDimension('measure', 2, 2)] self.array2.data = [0.0] * 2 # Initialisation of the matrices self.A = numpy.mat([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) self.P = numpy.mat([[1, 0, 0], [0, 1, 0], [0, 0, 3.5]]) self.Q = numpy.mat([[pow(0.5, 2), 0, 0], [0, pow(0.5, 2), 0], [0, 0, 0.001]]) self.H = numpy.mat([[1, 0, 0], [0, 1, 0]]) self.R = numpy.mat([[0.1, 0], [0, 0.1]]) self.U = numpy.vstack([0.0, 0.0]) # State is x, y, theta self.state = numpy.vstack([0.0, 0.0, 0.0]) self.last_update = rospy.Time.now()
def callback(self, joint_state): if len(self.joint_position) == 0: self.joint_position = np.array(joint_state.position) else: if np.all(self.joint_position == joint_state.position): return else: self.joint_position = copy.deepcopy(joint_state.position) print self.joint_position mat = UInt16MultiArray() mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim[0].label = "servo_number" mat.layout.dim[0].size = 6 mat.layout.dim[0].stride = 3 * 6 mat.layout.dim[1].label = "single_command" mat.layout.dim[1].size = 3 mat.layout.dim[1].stride = 3 mat.layout.data_offset = 0 mat.data = [ 1, 1500, 1000, 2, 1500, 1000, 3, 1500, 1000, 4, 1500, 1000, 5, 1500, 1000, 6, 1500, 1000 ] dstride1 = mat.layout.dim[1].stride offset = mat.layout.data_offset for i in range(6): if i == 1 or i == 2: PMW_value = -self.joint_position[i] / 1.5708 * 1000 + 1500 else: PMW_value = self.joint_position[i] / 1.5708 * 1000 + 1500 mat.data[offset + 1 + i * dstride1] = PMW_value print mat self.PMW_value_pub.publish(mat)
def take_pictures_test(self): #input: None #output: None #Description: Used for validating, AI is working with camera or not #Getting the bird eye view of the image #persImg = perspective(cv_image) persImg = skimage.io.imread(INPUT_TEST_IMAGE_PATH) objCnt,objCord = self.weedLocation(persImg) #Making up the object location data in format for publishing if objCnt !=0: objCord = np.reshape(objCord,(objCnt*3,)) #reshaping in the form of single column objCord = np.ndarray.tolist(objCord) else: objCord = np.reshape(objCord,(3,)) #reshaping in the form of single column objCord = np.ndarray.tolist(objCord) msg = Float32MultiArray() msg.layout.dim.append(MultiArrayDimension()) msg.layout.dim.append(MultiArrayDimension()) msg.layout.dim[0].label = "row" msg.layout.dim[1].label = "col" msg.layout.dim[0].size = objCnt msg.layout.dim[1].size = 1 msg.layout.dim[0].stride = 3 msg.layout.dim[1].stride = 1 msg.layout.data_offset = 0 msg.data = objCord self.publisher.publish(msg)
def think(msgs): global curx, cury, positions, r print(msgs.data) data = [int(x) for x in msgs.data] idxs = sorted(range(9), key=lambda k:data[k]) mat = Float32MultiArray() mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim[0].label = "height" mat.layout.dim[1].label = "width" mat.layout.dim[0].size = 2 mat.layout.dim[1].size = 9 mat.data = [0]*18 print(idxs) for i in range(len(idxs)): pos = positions[idxs[i]] print(pos) tx = pos[0] - curx ty = pos[1] - cury curx = pos[0] cury = pos[1] mat.data[i * 2] = tx mat.data[i * 2 + 1] = ty print(mat) pub.publish(mat) r.sleep()
def to_multiarray(img_shape, boxes, scores, classes): # image_shape : the shape of the image used by yolo # boxes: An `array` of shape (num_boxes, 4) containing box corners as # (y_min, x_min, y_max, x_max). # `scores`: A `list` of scores for each box. # classes: A `list` of indicies into `class_names`. # return: multiarray data = [] for i in range(len(boxes)): y_min = boxes[i][0] / img_shape[0] x_min = boxes[i][1] / img_shape[1] y_max = boxes[i][2] / img_shape[0] x_max = boxes[i][3] / img_shape[1] confidence = scores[i] _class = classes[i] data.append(float(_class)) data.append(float(y_min)) data.append(float(x_min)) data.append(float(y_max)) data.append(float(x_max)) data.append(float(confidence)) message = Float32MultiArray() message.layout.dim.append(MultiArrayDimension("boxes", len(boxes), 1)) message.layout.dim.append(MultiArrayDimension("box_data", 6, 1)) message.data = data return message
def test_ros_message_with_child_message(self): from std_msgs.msg import Float64MultiArray, MultiArrayLayout, MultiArrayDimension expected_dictionary = { 'layout': { 'dim': [{ 'label': 'Dimension1', 'size': 12, 'stride': 7 }, { 'label': 'Dimension2', 'size': 24, 'stride': 14 }], 'data_offset': 0 }, 'data': [1.1, 2.2, 3.3] } dimension1 = MultiArrayDimension( label=expected_dictionary['layout']['dim'][0]['label'], size=expected_dictionary['layout']['dim'][0]['size'], stride=expected_dictionary['layout']['dim'][0]['stride']) dimension2 = MultiArrayDimension( label=expected_dictionary['layout']['dim'][1]['label'], size=expected_dictionary['layout']['dim'][1]['size'], stride=expected_dictionary['layout']['dim'][1]['stride']) layout = MultiArrayLayout( dim=[dimension1, dimension2], data_offset=expected_dictionary['layout']['data_offset']) message = Float64MultiArray(layout=layout, data=expected_dictionary['data']) message = serialize_deserialize(message) dictionary = message_converter.convert_ros_message_to_dictionary( message) self.assertEqual(dictionary, expected_dictionary)
def callback(msg, prevMarkers): detectedMarkers = msg.markers # The current time in seconds now = rospy.get_time() for detectedMarker in detectedMarkers: measuredTime = detectedMarker.header.stamp.secs markerID = detectedMarker.id prevMarkers[markerID] = measuredTime detected_markers = list() for marker in prevMarkers.keys(): if abs(prevMarkers[marker] - now) > 5: del prevMarkers[marker] else: detected_markers.append(marker) array = MultiArrayDimension() array.label = 'numMarkers' array.size = len(detected_markers) array.size = len(detected_markers) layout = MultiArrayLayout() layout.dim = [ array, ] layout.data_offset = 0 msg = Int16MultiArray() msg.layout = layout msg.data = detected_markers pub.publish(msg)
def image_to_saliency(t, image, bridge, saliency, saliency_pub, saliency_image_pub, points, camera_model, camera_info_left, last_time, elapsed, pan, tilt, saliency_map): if image.value is None or camera_info_left.value is None: return if last_time.value is None: last_time.value = t current_time = t dt = current_time - last_time.value last_time.value = current_time elapsed.value = elapsed.value + dt if elapsed.value < 0.01: return else: elapsed.value = 0. if saliency_map.value is None: image = bridge.value.imgmsg_to_cv2(image.value, "bgr8") image = np.zeros(image.shape) saliency_map.value = saliency.value.compute_saliency_map(image) saliency_map_current = saliency_map.value.copy() # apply curiosity camera_model.value.fromCameraInfo(camera_info_left.value) for point in points.value: # call service pixel = camera_model.value.project3dToPixel( (point.point.x - pan.value, point.point.y - tilt.value, point.point.z)) x = int(pixel[0] * (len(saliency_map_current[0]) / float(camera_info_left.value.width))) x = x + 6 # correction, bug in opencv? y = int( pixel[1] * (len(saliency_map_current) / float(camera_info_left.value.height))) if x >= 0 and x < len(saliency_map_current[0]) and y >= 0 and y < len( saliency_map_current): from skimage.draw import circle rr, cc = circle( y, x, 25, (len(saliency_map_current), len(saliency_map_current[0]))) saliency_map[rr, cc] = saliency_map[rr, cc] * min( 1, (t - point.header.stamp.to_sec())) saliency_map_image = bridge.value.cv2_to_imgmsg( np.uint8(saliency_map_current * 255.), "mono8") saliency_image_pub.value.publish(saliency_map_image) from std_msgs.msg import Float32MultiArray, MultiArrayDimension, MultiArrayLayout height = MultiArrayDimension(size=len(saliency_map_current)) width = MultiArrayDimension(size=len(saliency_map_current[0])) lo = MultiArrayLayout([height, width], 0) saliency_pub.value.publish( Float32MultiArray(layout=lo, data=saliency_map_current.flatten())) return
def bno055_pub(): rospy.init_node('bno055_pub', anonymous=True) pub = rospy.Publisher('bno055_state', Float32MultiArray, queue_size=1) rate = rospy.Rate(10) # 10hz bno_controller = AdafruitBno055Controller() mat = Float32MultiArray() mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim.append(MultiArrayDimension()) mat.layout.dim[0].label = "height" mat.layout.dim[1].label = "width" mat.layout.dim[0].size = 3 mat.layout.dim[1].size = 1 mat.layout.dim[0].stride = 3 * 1 #note dim[0] stride is just size of image mat.layout.dim[1].stride = 1 mat.layout.data_offset = 0 mat.data = [0] * 3 while not rospy.is_shutdown(): x, y, z = bno_controller.gravity() gravity_str = "gravity is{}".format([x, y, z]) rospy.loginfo(gravity_str) mat.data[0] = x mat.data[1] = y mat.data[2] = z pub.publish(mat) rate.sleep()
def publish(self): # compose the multiarray message jointVelocities = Float32MultiArray() myLayout = MultiArrayLayout() myMultiArrayDimension = MultiArrayDimension() myMultiArrayDimension.label = "joint_velocities" myMultiArrayDimension.size = 1 myMultiArrayDimension.stride = self.numOfWheels myLayout.dim = [myMultiArrayDimension] myLayout.data_offset = 0 jointVelocities.layout = myLayout if rospy.get_time() - self.lastTwistTime < self.timeout: self.right = 1.0 * self.linearVelocity + self.angularVelocity * self.baseWidth / 2 self.left = 1.0 * self.linearVelocity - self.angularVelocity * self.baseWidth / 2 rospy.loginfo("wheels velocities vl, vr [{0:.3f},{1:.3f}]".format(self.left, self.right)) if self.numOfWheels == 2: # first item is left and second is right jointVelocities.data = [self.left, self.right] elif self.numOfWheels == 4: jointVelocities.data = [self.left, self.right, self.left, self.right] else: if self.numOfWheels == 2: jointVelocities.data = [0.0, 0.0] # first item is left and second is right elif self.numOfWheels == 4: jointVelocities.data = [0.0, 0.0, 0.0, 0.0] self.joint_velocities_Pub.publish(jointVelocities)
def feedback(feedback): global pub, initial_joint_position, fixed_frame, joint_names, group_name server.setPose(feedback.marker_name, feedback.pose) server.applyChanges() if feedback.event_type == 0: return rospy.wait_for_service('compute_ik') try: service = rospy.ServiceProxy('compute_ik', GetPositionIK) request = GetPositionIKRequest() request.ik_request.group_name = group_name request.ik_request.timeout = rospy.Duration.from_sec(0.0001) # initial robot state robot_state = RobotState() robot_state.joint_state.header.frame_id = fixed_frame robot_state.joint_state.name = joint_names robot_state.joint_state.position = initial_joint_position robot_state.joint_state.velocity = [] request.ik_request.robot_state = robot_state # goal end pose pose_stamped = PoseStamped() pose_stamped.header.frame_id = fixed_frame pose_stamped.pose.position.x = feedback.pose.position.x pose_stamped.pose.position.y = feedback.pose.position.y pose_stamped.pose.position.z = feedback.pose.position.z pose_stamped.pose.orientation.x = feedback.pose.orientation.x pose_stamped.pose.orientation.y = feedback.pose.orientation.y pose_stamped.pose.orientation.z = feedback.pose.orientation.z pose_stamped.pose.orientation.w = feedback.pose.orientation.w request.ik_request.pose_stamped = pose_stamped response = service(request) print response if len(response.solution.joint_state.position) != 0: print "success" msg = Float64MultiArray() for i,joint_name in enumerate(response.solution.joint_state.name): for j, name in enumerate(joint_names): if joint_name == name: initial_joint_position[j] = response.solution.joint_state.position[i] dim = MultiArrayDimension() dim.label = name msg.layout.dim.append(dim) msg.data.append(response.solution.joint_state.position[i]) pub.publish(msg) except rospy.ServiceException, e: print "Service call failed: %s"%e
def make_bounding_box_msg(bbox, selected=False): bb = Float64MultiArray() bb.layout.data_offset = 0 dim = MultiArrayDimension() dim.label = "bb" if not selected else "select" # we use the label of the first dimension to carry the selected/not selected infomation bb.layout.dim = [dim] x_min, y_min, x_max, y_max = bbox bb.data = [x_min, y_min, x_max, y_max] return bb
def talker(): torso_pub = rospy.Publisher('/torso_controller/command', Float64MultiArray, queue_size=10) left_leg_pub = rospy.Publisher('/left_leg_controller/command', Float64MultiArray, queue_size=10) right_leg_pub = rospy.Publisher('/right_leg_controller/command', Float64MultiArray, queue_size=10) rospy.init_node('publish_to_controller', anonymous=True) rate = rospy.Rate(1) msg = Float64MultiArray() # hipyaw, hiproll, hippitch, kneepitch, anklepitch, ankleroll msg.data = [0.0, 0.0, -0.63, 1.03, -0.44, 0.0] # msg.data = [0.0, 0.0, 0., 0.0, 0., 0.0] dim = MultiArrayDimension() dim.size = len(msg.data) dim.label = "command" dim.stride = len(msg.data) msg.layout.dim.append(dim) msg.layout.data_offset = 0 torso_msg = Float64MultiArray() torso_msg.data = [0.0, 0.2, 0.0] dim = MultiArrayDimension() dim.size = len(torso_msg.data) dim.label = "command" dim.stride = len(torso_msg.data) torso_msg.layout.dim.append(dim) torso_msg.layout.data_offset = 0 while not rospy.is_shutdown(): print msg left_leg_pub.publish(msg) right_leg_pub.publish(msg) torso_pub.publish(torso_msg) rate.sleep()
def makeMultiArray(iterable, label): arrayList = [] for el in iterable: arrayList.append(el) dim = MultiArrayDimension() dim.size = len(arrayList) dim.label = label dim.stride = len(arrayList) tempArray = Float64MultiArray() tempArray.data = arrayList tempArray.layout.dim.append(dim) tempArray.layout.data_offset = 0 return tempArray
def publish(self): #pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=10) pub = rospy.Publisher('remote_readings', Float64MultiArray, queue_size=1) rate = rospy.Rate(20) #10Hz a = [0.0, 0.0] dim = MultiArrayDimension() dim.size = len(a) dim.label = "REMOTEmsg" dim.stride = len(a) apub = Float64MultiArray() apub.data = a apub.layout.dim.append(dim) apub.layout.data_offset = 0 cnt=0 while not rospy.is_shutdown(): begin = time.time() #UART_read = ser.readline() #UART_read = ser.Serial.readline() #self.dev.flushInput() while self.dev.inWaiting() < 40: pass UART_read = self.dev.readline() self.dev.flushInput() #if self.dev.inWaiting() > 80: # self.dev.flushInput() #if self.dev.inWaiting() > 50: # self.dev.flushInput() # cnt=0 #self.dev.flushInput() #rospy.loginfo(UART_read) steer_cmd = int(UART_read[0:4]) throt_cmd = int(UART_read[4:8]) #self.dev.flushInput() self.dev1.pulsewidth_us(throt_cmd) self.dev2.pulsewidth_us(steer_cmd) a = [throt_cmd, steer_cmd] rospy.loginfo(a) apub.data = a pub.publish(apub) buffer = self.dev.inWaiting() #rospy.loginfo(buffer) #cnt=cnt+1 #self.dev.flushInput() #length = time.time() - begin #rospy.loginfo(length) rate.sleep()
def __init__(self): pub1 = rospy.Publisher('tactile', UBI0All) pub2 = rospy.Publisher('palm_extras', Float64MultiArray) pub3 = rospy.Publisher('tactile_mid_prox', MidProxDataAll) pub4 = rospy.Publisher('tactile_aux_spi', AuxSpiData) ubiall = UBI0All() ubiall.tactiles=[] ubi= UBI0() ubi.distal = [200, 300, 200, 300, 400, 500, 600, 700, 800, 900, 1000, 900] midproxall = MidProxDataAll() midproxall.sensors=[] midprox = MidProxData() midprox.middle = [1500, 2400, 0, 0] midprox.proximal = [100, 400, 800, 0] for iFinger in range(0,5): ubiall.tactiles.append(ubi) midproxall.sensors.append(midprox) palmextras = Float64MultiArray() dim = MultiArrayDimension() dim.label = "accelerometer" dim.size = 3 palmextras.layout.dim.append(dim) dim = MultiArrayDimension() dim.label = "gyrometer" dim.size = 3 palmextras.layout.dim.append(dim) dim = MultiArrayDimension() dim.label = "analog_inputs" dim.size = 4 palmextras.layout.dim.append(dim) palmextras.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 250.0, 550.0,850.0, 1150.0] auxspi = AuxSpiData() auxspi.sensors = [100, 400, 700, 1000, 1300, 1700, 2000, 2300, 100, 200, 300, 400, 500, 600, 700, 800] rospy.init_node('agni_tactile_test_sensors') r = rospy.Rate(100) # 100hz while not rospy.is_shutdown(): pub1.publish(ubiall) pub2.publish(palmextras) pub3.publish(midproxall) pub4.publish(auxspi) r.sleep()
def __init__(self): """ The constructor. """ # Define the joint info self.jointSpecs = [] self.jointSpecs.append(NeckJointDetails("lower_neck_pitch", 0, -40, 15)) self.jointSpecs.append(NeckJointDetails("upper_neck_yaw", 1, -80, 80)) self.jointSpecs.append(NeckJointDetails("upper_neck_roll", 2, -14, 14)) self.jointSpecs.append(NeckJointDetails("upper_neck_pitch", 3, -7, 37)) self.jointSpecs.append(NeckJointDetails("eye_pitch", 4, -34, 34)) self.jointSpecs.append(NeckJointDetails("right_eye_yaw", 5, -34, 34)) self.jointSpecs.append(NeckJointDetails("left_eye_yaw", 6, -34, 34)) # Define the goal message # Create the goal message. Store the current joint states into the messages dimPos = MultiArrayDimension() dimPos.size = len(self.jointSpecs) dimPos.label = "goalPosMsg" dimPos.stride = 1 self.goalPosMsg = Float64MultiArray() for ii in range(0, len(self.jointSpecs)): self.goalPosMsg.data.append(0) self.goalPosMsg.layout.dim.append(dimPos) self.goalPosMsg.layout.data_offset = 0 # dimVel = MultiArrayDimension() # dimVel.size = len(self.jointSpecs) # dimVel.label = "goalVelMsg" # dimVel.stride = 1 # self.goalVelMsg = Float64MultiArray() # for ii in range(0, len(self.jointSpecs)): # self.goalVelMsg.data.append(0) # self.goalVelMsg.layout.dim.append(dimVel) # self.goalVelMsg.layout.data_offset = 0 self.cmdPublisher = rospy.Publisher("/dreamer_controller/controlit/head/position_cmd", Float64MultiArray, queue_size=1)
def __init__(self): # Define the dimensions of the message dim = MultiArrayDimension() dim.size = NUM_DOFS dim.label = "rightHandOrientationGoal" dim.stride = 1 # Define the goal messages self.rightHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.rightHandGoalMsg.data.append(0) self.rightHandGoalMsg.layout.dim.append(dim) self.rightHandGoalMsg.layout.data_offset = 0 self.rightHandGoalMsg.data[2] = 1 # initial goal is [0, 0, 1] in world frame self.leftHandGoalMsg = Float64MultiArray() for ii in range(0, NUM_DOFS): self.leftHandGoalMsg.data.append(0) self.leftHandGoalMsg.layout.dim.append(dim) self.leftHandGoalMsg.layout.data_offset = 0 self.leftHandGoalMsg.data[2] = 1 # initial goal is [0, 0, 1] in world frame
def __init__(self, rosTopic, amplitude, offset, initGoal, numDoFs, jointIndex, period, updateFreq): """ The constructor. Keyword arguments: rosTopic -- The ROS topic on which to publish the goal. initGoal -- The initial goal (the value the sine wave starts at) amplitude -- The amplitude. offset -- The offset. numDoFs -- The number of DoFs. jointIndex -- The index of the joint being controlled. period -- The period of the sine wave in seconds. updateFreq -- The frequency at which the reference position should be sent """ self.period = period self.frequency = 1.0 / period self.rosTopic = rosTopic self.amplitude = amplitude self.offset = offset self.numDoFs = numDoFs self.jointIndex = jointIndex self.updateFreq = updateFreq self.updatePeriod = 1.0 / updateFreq # Define the dimensions of the message dim = MultiArrayDimension() dim.size = numDoFs dim.label = "goalMsg" dim.stride = 1 # Define the goal message self.goalMsg = Float64MultiArray() for ii in range(0, numDoFs): self.goalMsg.data.append(initGoal) self.goalMsg.layout.dim.append(dim) self.goalMsg.layout.data_offset = 0
def sa_right_set_callback(self): sa_right_cam = Float64MultiArray() sa_right_cam_dim_h = MultiArrayDimension() sa_right_cam_dim_h.label = "height" sa_right_cam_dim_h.size = 1 sa_right_cam_dim_h.stride = 2 sa_right_cam.layout.dim.append( sa_right_cam_dim_h ) sa_right_cam_dim_w = MultiArrayDimension() sa_right_cam_dim_w.label = "width" sa_right_cam_dim_w.size = 2 sa_right_cam_dim_w.stride = 2 sa_right_cam.layout.dim.append(sa_right_cam_dim_w) sa_right_cam.data.append( self.sa_right_gain.value() ) sa_right_cam.data.append( self.sa_right_exp.value() ) self.sa_right_cam_pub.publish( sa_right_cam )
def ms_set_callback(self): ms_cam = Float64MultiArray() ms_cam_dim_h = MultiArrayDimension() ms_cam_dim_h.label = "height" ms_cam_dim_h.size = 1 ms_cam_dim_h.stride = 2 ms_cam.layout.dim.append( ms_cam_dim_h ) ms_cam_dim_w = MultiArrayDimension() ms_cam_dim_w.label = "width" ms_cam_dim_w.size = 2 ms_cam_dim_w.stride = 2 ms_cam.layout.dim.append(ms_cam_dim_w) ms_cam.data.append( self.ms_gain.value() ) if self.ms_exp_auto.isChecked() : ms_cam.data.append( 1 ) else: ms_cam.data.append( self.ms_exp.value() ) self.ms_cam_pub.publish( ms_cam ) light = Float64() light.data = self.ms_light.value()/100.0 self.ms_light_pub.publish( light ) spindle_speed = Float64() spindle_speed.data = self.ms_spindle.value() self.ms_spindle_pub.publish( spindle_speed )
def __init__(self, enableUserPrompts = False, useQuaternionControl = False): ''' The constructor. Keyword arguments: - enableUserPrompts: Whether to enable user prompts (default is False) - userQuaternionControl: Whether to use quaternions for orientation control (default is False) ''' self.enableUserPrompts = enableUserPrompts self.useQuaternionControl = useQuaternionControl if self.useQuaternionControl: NUM_ORIENTATION_DOFS = 4 # Orientation is defined using a w, x, y, z vector else: NUM_ORIENTATION_DOFS = 3 # Orientation is defined using a x, y, z vector # Define the goal messages rightHandCartesianGoalMsgDim = MultiArrayDimension() rightHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS rightHandCartesianGoalMsgDim.label = "rightHandCartesianGoal" rightHandCartesianGoalMsgDim.stride = 1 self.rightHandCartesianGoalMsg = Float64MultiArray() for ii in range(0, NUM_CARTESIAN_DOFS): self.rightHandCartesianGoalMsg.data.append(0) self.rightHandCartesianGoalMsg.layout.dim.append(rightHandCartesianGoalMsgDim) self.rightHandCartesianGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' leftHandCartesianGoalMsgDim = MultiArrayDimension() leftHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS leftHandCartesianGoalMsgDim.label = "leftHandCartesianGoal" leftHandCartesianGoalMsgDim.stride = 1 self.leftHandCartesianGoalMsg = Float64MultiArray() for ii in range(0, NUM_CARTESIAN_DOFS): self.leftHandCartesianGoalMsg.data.append(0) self.leftHandCartesianGoalMsg.layout.dim.append(leftHandCartesianGoalMsgDim) self.leftHandCartesianGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' if not self.useQuaternionControl: rightHandOrientationGoalMsgDim = MultiArrayDimension() rightHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS rightHandOrientationGoalMsgDim.label = "rightHandOrientationGoal" rightHandOrientationGoalMsgDim.stride = 1 self.rightHandOrientationGoalMsg = Float64MultiArray() for ii in range(0, NUM_ORIENTATION_DOFS): self.rightHandOrientationGoalMsg.data.append(0) self.rightHandOrientationGoalMsg.layout.dim.append(rightHandOrientationGoalMsgDim) self.rightHandOrientationGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' leftHandOrientationGoalMsgDim = MultiArrayDimension() leftHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS leftHandOrientationGoalMsgDim.label = "leftHandOrientationGoal" leftHandOrientationGoalMsgDim.stride = 1 self.leftHandOrientationGoalMsg = Float64MultiArray() for ii in range(0, NUM_ORIENTATION_DOFS): self.leftHandOrientationGoalMsg.data.append(0) self.leftHandOrientationGoalMsg.layout.dim.append(leftHandOrientationGoalMsgDim) self.leftHandOrientationGoalMsg.layout.data_offset = 0 else: self.orientationUpdateMsg = QuatInterpMsg() #-----------------------------------------------------------------------------' postureGoalMsgDim = MultiArrayDimension() postureGoalMsgDim.size = NUM_ROBOT_DOFS postureGoalMsgDim.label = "postureGoal" postureGoalMsgDim.stride = 1 self.postureGoalMsg = Float64MultiArray() for ii in range(0, NUM_ROBOT_DOFS): self.postureGoalMsg.data.append(0) self.postureGoalMsg.layout.dim.append(postureGoalMsgDim) self.postureGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' self.rightHandCmdMsg = Bool() self.rightHandCmdMsg.data = False # relax hand self.rightIndexFingerCmdMsg = Bool() self.rightIndexFingerCmdMsg.data = True # include index finger in power grasp self.rightMiddleFingerCmdMsg = Bool() self.rightMiddleFingerCmdMsg.data = True # include middle finger in power grasp self.rightPinkyFingerCmdMsg = Bool() self.rightPinkyFingerCmdMsg.data = True # include pinky finger in power grasp self.leftGripperCmdMsg = Bool() self.leftGripperCmdMsg.data = False # relax gripper self.tareMsg = Int32() self.tareMsg.data = 1 self.enableMsg = Int32() self.enableMsg.data = 1 #-----------------------------------------------------------------------------' # Initialize member variables self.currentPosture = None self.postureError = None self.currentRightCartesianPos = None self.rightCartesianPosError = None self.currentLeftCartesianPos = None self.leftCartesianPosError = None self.currentRightOrientation = None self.rightOrientationError = None self.currentLeftOrientation = None self.leftOrientationError = None # Create the ROS topic subscriptions self.postureTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/Posture/actualPosition", Float64MultiArray, self.postureTaskActualCallback) self.postureTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/Posture/error", Float64MultiArray, self.postureTaskErrorCallback) self.rightCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/actualWorldPosition", Float64MultiArray, self.rightCartesianTaskActualCallback) self.rightCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/error", Float64MultiArray, self.rightCartesianTaskErrorCallback) self.leftCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandPosition/actualWorldPosition", Float64MultiArray, self.leftCartesianTaskActualCallback) self.leftCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandPosition/error", Float64MultiArray, self.leftCartesianTaskErrorCallback) if self.useQuaternionControl: self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualWorldOrientation", Float64MultiArray, self.rightOrientationTaskActualCallback) else: self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualHeading", Float64MultiArray, self.rightOrientationTaskActualCallback) self.rightOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/errorAngle", Float64, self.rightOrientationTaskErrorCallback) if self.useQuaternionControl: self.leftOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/actualWorldOrientation", Float64MultiArray, self.leftOrientationTaskActualCallback) else: self.leftOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/actualHeading", Float64MultiArray, self.leftOrientationTaskActualCallback) self.leftOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/LeftHandOrientation/errorAngle", Float64, self.leftOrientationTaskErrorCallback) # Create the ROS topic publishers self.postureTaskGoalPublisher = rospy.Publisher("/dreamer_controller/Posture/goalPosition", Float64MultiArray, queue_size=1) self.postureTaskTarePublisher = rospy.Publisher("/dreamer_controller/Posture/tare", Int32, queue_size=1) self.rightCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/goalPosition", Float64MultiArray, queue_size=1) self.rightCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/enableState", Int32, queue_size=1) self.rightCartesianTaskTarePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/tare", Int32, queue_size=1) self.leftCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/goalPosition", Float64MultiArray, queue_size=1) self.leftCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/enableState", Int32, queue_size=1) self.leftCartesianTaskTarePublisher = rospy.Publisher("/dreamer_controller/LeftHandPosition/tare", Int32, queue_size=1) if not self.useQuaternionControl: self.rightOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/goalVector", Float64MultiArray, queue_size=1) self.rightOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/enableState", Int32, queue_size=1) self.rightOrientationTaskTarePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/tare", Int32, queue_size=1) if not self.useQuaternionControl: self.leftOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/goalVector", Float64MultiArray, queue_size=1) self.leftOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/enableState", Int32, queue_size=1) self.leftOrientationTaskTarePublisher = rospy.Publisher("/dreamer_controller/LeftHandOrientation/tare", Int32, queue_size=1) if self.useQuaternionControl: self.orientationGoalPublisher = rospy.Publisher("/dreamer_controller/trajectory_generator/update_orientation", QuatInterpMsg, queue_size=1) self.rightHandCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/powerGrasp", Bool, queue_size=1) self.selectIndexFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightIndexFinger", Bool, queue_size=1) self.selectMiddleFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightMiddleFinger", Bool, queue_size=1) self.selectPinkyFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightPinkyFinger", Bool, queue_size=1) self.leftGripperCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/leftGripper/powerGrasp", Bool, queue_size=1)
def __init__(self): # Define the goal messages rightHandCartesianGoalMsgDim = MultiArrayDimension() rightHandCartesianGoalMsgDim.size = NUM_CARTESIAN_DOFS rightHandCartesianGoalMsgDim.label = "rightHandCartesianGoal" rightHandCartesianGoalMsgDim.stride = 1 self.rightHandCartesianGoalMsg = Float64MultiArray() for ii in range(0, NUM_CARTESIAN_DOFS): self.rightHandCartesianGoalMsg.data.append(0) self.rightHandCartesianGoalMsg.layout.dim.append(rightHandCartesianGoalMsgDim) self.rightHandCartesianGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' rightHandOrientationGoalMsgDim = MultiArrayDimension() rightHandOrientationGoalMsgDim.size = NUM_ORIENTATION_DOFS rightHandOrientationGoalMsgDim.label = "rightHandOrientationGoal" rightHandOrientationGoalMsgDim.stride = 1 self.rightHandOrientationGoalMsg = Float64MultiArray() for ii in range(0, NUM_ORIENTATION_DOFS): self.rightHandOrientationGoalMsg.data.append(0) self.rightHandOrientationGoalMsg.layout.dim.append(rightHandOrientationGoalMsgDim) self.rightHandOrientationGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' postureGoalMsgDim = MultiArrayDimension() postureGoalMsgDim.size = NUM_ROBOT_DOFS postureGoalMsgDim.label = "postureGoal" postureGoalMsgDim.stride = 1 self.postureGoalMsg = Float64MultiArray() for ii in range(0, NUM_ROBOT_DOFS): self.postureGoalMsg.data.append(0) self.postureGoalMsg.layout.dim.append(postureGoalMsgDim) self.postureGoalMsg.layout.data_offset = 0 #-----------------------------------------------------------------------------' self.rightHandCmdMsg = Bool() self.rightHandCmdMsg.data = False # relax hand self.rightIndexFingerCmdMsg = Bool() self.rightIndexFingerCmdMsg.data = True # include index finger in power grasp self.rightMiddleFingerCmdMsg = Bool() self.rightMiddleFingerCmdMsg.data = True # include middle finger in power grasp self.enableMsg = Int32() self.enableMsg.data = 1 #-----------------------------------------------------------------------------' # Initialize member variables self.currentPosture = None self.postureError = None self.currentRightCartesianPos = None self.rightCartesianPosError = None self.currentRightOrientation = None self.rightOrientationError = None # Create the ROS topic subscriptions self.postureTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/JPosTask/actualPosition", Float64MultiArray, self.postureTaskActualCallback) self.postureTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/JPosTask/error", Float64MultiArray, self.postureTaskErrorCallback) self.rightCartesianTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/actualWorldPosition", Float64MultiArray, self.rightCartesianTaskActualCallback) self.rightCartesianTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandPosition/error", Float64MultiArray, self.rightCartesianTaskErrorCallback) self.rightOrientationTaskActualSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/actualHeading", Float64MultiArray, self.rightOrientationTaskActualCallback) self.rightOrientationTaskErrorSubscriber = rospy.Subscriber("/dreamer_controller/RightHandOrientation/errorAngle", Float64, self.rightOrientationTaskErrorCallback) # Create the ROS topic publishers self.postureTaskGoalPublisher = rospy.Publisher("/dreamer_controller/JPosTask/goalPosition", Float64MultiArray, queue_size=1) self.rightCartesianTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/goalPosition", Float64MultiArray, queue_size=1) self.rightCartesianTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandPosition/enabled", Int32, queue_size=1) self.rightOrientationTaskGoalPublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/goalVector", Float64MultiArray, queue_size=1) self.rightOrientationTaskEnablePublisher = rospy.Publisher("/dreamer_controller/RightHandOrientation/enabled", Int32, queue_size=1) self.rightHandCmdPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/powerGrasp", Bool, queue_size=1) self.selectIndexFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightIndexFinger", Bool, queue_size=1) self.selectMiddleFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightMiddleFinger", Bool, queue_size=1) self.selectPinkyFingerPublisher = rospy.Publisher("/dreamer_controller/controlit/rightHand/includeRightPinkyFinger", Bool, queue_size=1)