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 __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 __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 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 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 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 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 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 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 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 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 __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 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 __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_state(self, state, labels): message = Int32MultiArray() #message.header.frame_id = REFERENCE_FRAME #message.header.stamp = rospy.Time(0) dim1 = MultiArrayDimension() dim2 = MultiArrayDimension() dim1.label = labels[0] dim1.size = state.shape[0] dim1.stride = state.size message.layout.dim.append(dim1) dim2.label = labels[1] dim2.size = state.shape[1] dim2.stride = state.shape[1] message.layout.dim.append(dim2) for a in state.flat[:]: message.data.append(a) self._state_pub.publish(message) pass
def make_hand_configuration(self, points): dim = MultiArrayDimension() dim.label = 'fingers' dim.size = 5 dim.stride = 5 msg = Float64MultiArray() msg.layout.dim = [dim] msg.layout.data_offset = 0 msg.data = points return msg
def sendCommand(self, ts_rostime): msg = Float64MultiArray() dim0 = MultiArrayDimension() dim0.label = 'ts_rostime, numOfsamples, dt' dim0.size = 3 layout_var = MultiArrayLayout() layout_var.dim = [dim0] msg.layout = layout_var msg.data = [ts_rostime, self.numOfSamples, self.dt] self.sampleParticalTrajectory_pub.publish(msg)
def pack_data(): msg = Float32MultiArray() msg.data = MySensors.get_meters() msg.layout.data_offset = 0 dim = MultiArrayDimension() dim.label = 'num_sensors' dim.size = 3 dim.stride = 1 msg.layout.dim = [dim] return msg
def pubLabelMsg(self, known_labels): msg = Int8MultiArray() dim = MultiArrayDimension() dim.label = "length" dim.size = len(known_labels) dim.stride = len(known_labels) msg.layout.dim.append(dim) msg.layout.data_offset = 0 msg.data = known_labels self.labels_pub.publish(msg)
def PublishtoPurePursuit(self, waypoints): pub_points = Float32MultiArray() wp_len = len(waypoints) dim0 = MultiArrayDimension() dim1 = MultiArrayDimension() dim0.label = "height" dim0.size = wp_len/3 dim1.label = "width" dim1.size = 3 dimensions = MultiArrayLayout() dimensions.dim = [dim0, dim1] pub_points.layout = dimensions pub_points.data = waypoints self.pursuit_pub.publish(pub_points)
def gen_msg(self, val): fa = Float64MultiArray() mad = MultiArrayDimension() mad.label = 'commands' mad.size = 2 mad.stride = 2 fa.layout.dim = [mad] fa.layout.data_offset = 0 fa.data = [val, val] return fa
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 callback(data): # tempolary hardcode the parameter x_lim = -10 y_lim = -10 x_width = 20 y_width = 20 resol = 0.1 map_meta = data.info N = int(x_width / resol) + 1 temp = np.ones((N, N)) * 50 dimh = MultiArrayDimension() dimw = dimh dimh.size = data.info.height dimh.label = 'height' dimh.stride = 48 dimw.size = data.info.width dimw.label = 'width' dimw.stride = 64 datanp = np.array(data.data, dtype=float) datanp = datanp datanp[datanp < 0] = 50 for i in range(data.info.height): for j in range(data.info.width): idx_x = round( N / 2 - map_meta.height / 2) #-data.info.origin.position.x/0.1) idx_y = round(N / 2 - -data.info.origin.position.x / 0.1) # temp[int(idx_x + i),int(idx_y + j)] = datanp[(i-1)*data.info.width+j] for i in range(80): for j in range(60): temp[int(N / 2 - 40 + i), int(N / 2 - 30 + j)] = 0.1 map = OccupancyGrid() map.data = temp.flatten() map.info.width = int(N) map.info.height = int(N) map.info.resolution = 0.1 map.info.origin = map_meta.origin # data.data pub = rospy.Publisher('/TheMap', Float64MultiArray, queue_size=10) pub_map = rospy.Publisher('/map_fixed', OccupancyGrid, queue_size=10) a = Float64MultiArray() a.layout.dim.append(dimh) a.layout.dim.append(dimw) a.layout.data_offset = 0 a.data = temp.flatten() / 100. pub_map.publish(map) pub.publish(a) br = tf.TransformBroadcaster() br.sendTransform((-map_meta.origin.position.x - 10, -map_meta.origin.position.y - 10, 0.0), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "map", "world")
def cb_score(event): global solveResult ret=Bool();ret.data=True;pub_Y2.publish(ret) cb_master(event) score=Float32MultiArray() for sc in solveResult: d=MultiArrayDimension() d.label=sc d.size=len(solveResult[sc]) d.stride=1 print "score",d.label
def create_array_message(): array_dim_0 = MultiArrayDimension() array_dim_0.label = 'dim_0' array_dim_0.size = 4 array_dim_0.stride = 0 array_dim_1 = MultiArrayDimension() array_dim_1.label = 'dim_1' array_dim_1.size = len_faces array_dim_1.stride = 0 array_layout = MultiArrayDimension() array_layout.label = "face_array" array_message = Float32MultiArray() array_message.layout.dim = [array_dim_0, array_dim_1] array_message.layout.data_offset = 0 face_0 = [0.0, 1.0, 1.0, 200.0] face_1 = [1.0, 0.0, 1.0, 100.0] face_2 = [2.0, 1.0, 0.0, 300.0] face_3 = [3.0, 0.0, 0.0, 400.0] array_message.data = face_0 + face_1 + face_2 + face_3 return array_message
def matrixListToMultiarray(matrix): rows, columns = matrix.shape msg = Float64MultiArray() # Set up layout msg.layout.data_offset = 0 row_dim = MultiArrayDimension() row_dim.label = "rows" row_dim.size = rows row_dim.stride = columns * rows col_dim = MultiArrayDimension() col_dim.label = "cols" col_dim.size = columns col_dim.stride = columns msg.layout.dim = [row_dim, col_dim] msg.data = matrix.flatten().tolist() return msg
def create_depth_msg(depth_img): command = Float64MultiArray() layout = MultiArrayLayout() dimension = MultiArrayDimension() dimension.label = "depth_img" dimension.size = len(depth_img) dimension.stride = len(depth_img) layout.data_offset = 0 layout.dim = [dimension] command.layout = layout command.data = depth_img return command
def adjustLeftHand(position): Lmsg = Float64MultiArray() dim = MultiArrayDimension() dim.label = 'fingers' dim.size = 5 dim.stride = 5 Lmsg.layout.dim = [dim] Lmsg.layout.data_offset = 0 Lmsg.data = position left_hand_publisher.publish(Lmsg)
def adjustRightHand(position): Rmsg = Float64MultiArray() dim = MultiArrayDimension() dim.label = 'fingers' dim.size = 5 dim.stride = 5 Rmsg.layout.dim = [dim] Rmsg.layout.data_offset = 0 Rmsg.data = position right_hand_publisher.publish(Rmsg)
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 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 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 publish(vals): command = Float64MultiArray() layout = MultiArrayLayout() dimension = MultiArrayDimension() dimension.label = "keyboard_control" dimension.size = 4 dimension.stride = 4 layout.data_offset = 0 layout.dim = [dimension] command.layout = layout command.data = vals control_pub.publish(command)
def main(): thread1 = myThread() thread1.start() rospy.init_node('reconstruction_node', anonymous=True) # node_name: reconstruction_node pub = rospy.Publisher('topic_name', Float32MultiArray, queue_size=10) # topic_name: topic_name instance_multi_array = Float32MultiArray() instance_multi_array.layout.data_offset = 0 temp_a = MultiArrayDimension() temp_b = MultiArrayDimension() temp_a.label = 'LED' temp_a.size = np.size(thread1.reconstruction_data_array, 0) temp_a.stride = np.size(thread1.reconstruction_data_array, 1) * np.size( thread1.reconstruction_data_array, 0) temp_b.label = 'Sensor' temp_b.size = np.size(thread1.reconstruction_data_array, 1) temp_b.stride = 1 * np.size(thread1.reconstruction_data_array, 1) instance_multi_array.layout.dim.append(temp_a) instance_multi_array.layout.dim.append(temp_b) temp_c = np.reshape(thread1.reconstruction_data_array, (1, thread1.reconstruction_data_array.size), 'C') instance_multi_array.data = temp_c[0] rate = rospy.Rate(1) rospy.loginfo("Node_Publishes") while not rospy.is_shutdown(): # rospy.loginfo(instance_multi_array) pub.publish(instance_multi_array) rate.sleep()
def publisher(recieved): pub = rospy.Publisher('joint_goals', Float64MultiArray, queue_size=10) rospy.loginfo('Publishing the goal') rospy.loginfo(recieved) published_msg = Float64MultiArray() multi_dim = MultiArrayDimension() multi_dim.label = 'foo' multi_dim.size = 1 multi_dim.stride = 1 published_msg.layout.dim = [multi_dim] published_msg.layout.data_offset = 1 published_msg.data = recieved.position pub.publish(published_msg)
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 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 make_multi_array(iterable, label): array_list = [] for el in iterable: array_list.append(el) dim = MultiArrayDimension() dim.size = len(array_list) dim.label = label dim.stride = len(array_list) temp_array = Int16MultiArray() temp_array.data = array_list temp_array.layout.dim.append(dim) temp_array.layout.data_offset = 0 return temp_array
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): self.hand_joint_value = [10, 10, 0, 0, 0] rospy.Subscriber("hand_joint_state_pub", Float64MultiArray, self.sensor_call_back) fingertip_data = rospy.Publisher('fingertip_state', Float64MultiArray, queue_size=10) joint_displacement_array = rospy.Publisher( 'hand_controller/joint_group_position_controller/command', Float64MultiArray, queue_size=10) data_send = Float64MultiArray() displacement_send = Float64MultiArray() rospy.init_node('hand_joint_state_pub_sim', anonymous=True) rate = rospy.Rate(30) # 10hz while not rospy.is_shutdown(): new_fingertip_state = kclhand_fingertip_state( palmJointA=-self.hand_joint_value[0], palmJointE=-self.hand_joint_value[1], leftFingerLower=-self.hand_joint_value[2], middleFingerLower=-self.hand_joint_value[3], rightFingerLower=self.hand_joint_value[4]) fingertip = new_fingertip_state.get_fingertip_data() #displacement_send.name = new_fingertip_state.kclhand_joint_name displacement_send.data = new_fingertip_state.get_joint_displacement( ) #displacement_send.position = [0,0,0,0,0,0,0,0,0,0] dim = MultiArrayDimension() dim.label = "fingertip_position" dim.size = 9 dim.stride = 9 data_send.layout.data_offset = 0 #data_send.layout.dim.append(dim) data_send.data = [] for i in range(0, 8): data_send.data.append(fingertip[i]) fingertip_data.publish(data_send) joint_displacement_array.publish(displacement_send) rate.sleep()
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 makeMultiArray(gripper_left, gripper_right): arrayList = [] arrayList.append(gripper_left) arrayList.append(gripper_right) dim = MultiArrayDimension() dim.size = 2 dim.label = '' dim.stride = 1 tempArray = Float64MultiArray() tempArray.data = arrayList tempArray.layout.dim.append(dim) tempArray.layout.data_offset = 0 return tempArray
def pub_voltage(self): t = time.time() arr = np.array([[525330, 0, 2.5 + 2.5*np.sin(0.5*t)], [525330, 1, 2.5 + 2.5*np.sin(0.7*t+np.pi/3.)], [525330, 2, 2.5 + 2.5*np.sin(0.9*t+np.pi/2.)],]) msg = Float32MultiArray(data=np.ravel(arr)) d = MultiArrayDimension() d.label = "shape" d.size = arr.shape[0] d.stride = arr.shape[1] msg.layout.dim = [d] self.control_pub.publish(msg)
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 __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)
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)