def state_publisher(self): rate = rospy.Rate(1.0 / self.dt) while not rospy.is_shutdown(): time = rospy.Time.now() # write to JointState header = Header(stamp=time) msg = JointState(header=header, name=self.lnames, position=self.joints, velocity=numpy.zeros(self.dof), effort=numpy.zeros(self.dof)) self.joint_publisher.publish(msg) msg = Float64MultiArray() msg.data = self.joints self.joint_publisher2.publish(msg) # write to tf t = [] for k in range(0, self.dof): t = t + [ pack_pose(time, self.links[k + 1], self.links[k], matrix=self.transformations[k](self.joints[k])) ] self.tf_publisher.sendTransform(t) rate.sleep()
def updatePosition(): global setpoint while not rospy.is_shutdown(): position = int( subprocess.check_output(["/home/scout3d/getMotorPosition", ""])) positionDeg = 90.0 * float(position) / 5000.0 positionRad = (math.pi / 2.0) * float(position) / 5000.0 msg = JointState() msg.header.stamp = rospy.Time.now() msg.header.frame_id = "/scanner_motor" msg.name = ["base_joint"] msg.position = [positionRad] msg.velocity = [0] msg.effort = [0] pubPosition.publish(msg) # print('position: ' + str(positionDeg) + ' (' + str(position) + ')') if not math.isnan(setpoint) and setpoint != position: # print('driving: ' + str(setpoint)) subprocess.check_output( ["/home/scout3d/setMotorPosition", str(setpoint)]) msg = Bool() msg.data = False pubStopped.publish(msg) else: # print('deenergize') subprocess.check_output(["/home/scout3d/off", ""]) msg = Bool() msg.data = True pubStopped.publish(msg) time.sleep(.01)
def makeROS(self, data): # Given a list of the form [{message type (str)}, {message data (dict)}] received via TCP, # parse and create a ROS message. print "Data received (" + str(sys.getsizeof(data[1])) + "): " + data[0] header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = RVIZ_FRAME if data[0] == '/hardware_joint_states': pass elif data[0] == '/ihmc_ros/valkyrie/output/joint_states': msg = JointState() msg.header = header msg.name = data[1]['name'] msg.position = data[1]['position'] self.joints_publisher.publish(msg) elif data[0] == '/tf': msg = TFMessage() msg.transforms = data[1]['transforms'] self.tf_publisher.publish(msg) elif data[0] == '/multisense/camera/points2': self.points2_publisher.publish(pc2.create_cloud_xyz32(header,data[1])) elif data[0] == '/multisense/camera/left/image_rect_color': msg = Image() msg.header = header msg.height = data[1]['height'] msg.width = data[1]['width'] msg.encoding = data[1]['encoding'] msg.is_bigendian = data[1]['is_bigendian'] msg.step = data[1]['step'] msg.data = data[1]['data'] self.image_publisher.publish(msg) bridge = CvBridge() left_image = cv2.flip(cv2.flip( bridge.imgmsg_to_cv2(msg, "bgr8") ,0),1) cv2.imshow("left camera",left_image) cv2.waitKey(3) else: print ("WARNING! Unrecognized data packet received. Ignoring data")
def state_publisher(self): rate = rospy.Rate(1.0/self.dt) while not rospy.is_shutdown(): time = rospy.Time.now() # write to JointState header = Header(stamp=time) msg = JointState(header=header, name=self.lnames, position=self.joints, velocity=numpy.zeros(self.dof), effort=numpy.zeros(self.dof)) self.joint_publisher.publish(msg) msg = Float64MultiArray() msg.data = self.joints self.joint_publisher2.publish( msg ) # write to tf t = [] for k in range(0, self.dof): t = t + [pack_pose(time, self.links[k+1], self.links[k], matrix=self.transformations[k](self.joints[k]))] self.tf_publisher.sendTransform(t) rate.sleep()