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()
