def airpub():
pub_left = rospy.Publisher("/gi/simulation/left/image_raw", Image, queue_size=1)
pub_right=rospy.Publisher("/gi/simulation/right/image_raw", Image, queue_size=1)
rospy.init_node('airsim_info', anonymous=True)
rate = rospy.Rate(10) # 10hz
# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
while not rospy.is_shutdown():
# get camera images from the car
Imu = client.getImuData()
responses = client.simGetImages([
airsim.ImageRequest("front-left", airsim.ImageType.Scene, False, False),
airsim.ImageRequest("front-right", airsim.ImageType.Scene, False, False)
])
img_dic={}
for idx, response in enumerate(responses):
img_rgb_string = response.image_data_uint8
img_dic[idx]=img_rgb_string
# Populate image message
msg_left=Image()
msg_left.header.stamp = rospy.Time.now()
msg_left.header.frame_id = "frameId"
msg_left.encoding = "rgb8"
msg_left.height = 480 # resolution should match values in settings.json
msg_left.width = 752
msg_left.data = img_dic[0]
msg_left.is_bigendian = 0
msg_left.step = msg_left.width * 3
msg_right = Image()
msg_right.header.stamp = rospy.Time.now()
msg_right.header.frame_id = "frameId"
msg_right.encoding = "rgb8"
msg_right.height = 480 # resolution should match values in settings.json
msg_right.width = 752
msg_right.data = img_dic[1]
msg_right.is_bigendian = 0
msg_right.step = msg_left.width * 3
# log time and size of published image
rospy.loginfo(len(response.image_data_uint8))
# publish image message
pub_left.publish(msg_left)
pub_right.publish(msg_right)
# sleep until next cycle
rate.sleep()
def main():
# img_height = 1208
# img_width = 1920 * 2
# _rate = 1
rospy.init_node("generate_fake_img", anonymous=True)
img_height = int(rospy.get_param('img_height', 1208))
img_width = int(rospy.get_param('img_width', 1920)) * 2
_rate = int(rospy.get_param('fps', 30))
pub = rospy.Publisher('simulate_camera_output_pub', Image, queue_size=10)
rate = rospy.Rate(_rate)
while not rospy.is_shutdown():
global count
count = count + 1
# img = generate_fake_img_random()
img = generate_pattern(img_height, img_width)
msg = Image()
msg.data = img.tostring()
msg.header.stamp = rospy.Time.now()
msg.height = img_height
msg.width = img_width / 2
msg.encoding = 'bayer_rggb16'
msg.is_bigendian = 0
msg.step = img_width
pub.publish(msg)
rate.sleep()
def _build_result_msg(self, msg, result):
result_msg = Result()
result_msg.header = msg.header
for i, (y1, x1, y2, x2) in enumerate(result['rois']):
box = RegionOfInterest()
box.x_offset = x1#np.asscalar(x1)
box.y_offset = y1#np.asscalar(y1)
box.height = y2-y1#np.asscalar(y2 - y1)
box.width = x2-x1#np.asscalar(x2 - x1)
result_msg.boxes.append(box)
class_id = result['class_ids'][i]
result_msg.class_ids.append(class_id)
class_name = self._class_names[class_id]
result_msg.class_names.append(class_name)
score = result['scores'][i]
result_msg.scores.append(score)
mask = Image()
mask.header = msg.header
mask.height = result['masks'].shape[0]
mask.width = result['masks'].shape[1]
mask.encoding = "mono8"
mask.is_bigendian = False
mask.step = mask.width
mask.data = (result['masks'][:, :, i] * 255).tobytes()
result_msg.masks.append(mask)
return result_msg
def _get_images(self):
# get camera images from the car
responses = self.client.simGetImages([
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False),
airsim.ImageRequest("2", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGBA array
msgs = []
stamp = rospy.Time.now()
for response in responses:
img_rgba_string = response.image_data_uint8
# Populate image message
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = "frameId"
msg.encoding = "rgba8"
msg.height = response.height
msg.width = response.width
msg.data = img_rgba_string
msg.is_bigendian = 0
msg.step = msg.width * 4
array = self.bridge.imgmsg_to_cv2(msg, "rgba8")
grayscale = cv2.cvtColor(array, cv2.COLOR_RGBA2GRAY)
converted = self.bridge.cv2_to_imgmsg(grayscale, "8UC1")
converted.header.stamp = stamp
msgs.append(converted)
assert(len(msgs) == 2)
rospy.loginfo(len(response.image_data_uint8))
return msgs
def airpub():
pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1)
rospy.init_node('image_raw', anonymous=True)
rate = rospy.Rate(10) # 10hz
# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
while not rospy.is_shutdown():
# get camera images from the car
responses = client.simGetImages([
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)
]) #scene vision image in uncompressed RGB array
for response in responses:
img_rgb_string = response.image_data_uint8
# Populate image message
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "frameId"
msg.encoding = "rgb8"
msg.height = 360 # resolution should match values in settings.json
msg.width = 640
msg.data = img_rgb_string
msg.is_bigendian = 0
msg.step = msg.width * 3
# log time and size of published image
rospy.loginfo(len(response.image_data_uint8))
# publish image message
pub.publish(msg)
# sleep until next cycle
rate.sleep()
def publish_processed_image(self):
# draw box and caption string
self.rgb = cv2.rectangle(self.rgb, (self.bbox[0], self.bbox[1]), (self.bbox[2],self.bbox[3]), (0,0,0xFF), 2)
self.rgb = cv2.putText(self.rgb, self.caption, (self.bbox[0]+2, self.bbox[1]-5), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0,0,0xFF), thickness=2)
#construct sensor_msgs/Image object
self.imgmsg.data = self.rgb.tobytes()
# publish frame
rospy.loginfo('publishing processed frame (with bbox) %s' % rospy.get_time())
self.rgb1_pub.publish(self.imgmsg)
# hack to republish frame
msk = np.zeros( (self.rgb.shape[0], self.rgb.shape[1]))
msk = cv2.rectangle(msk, (self.bbox[0], self.bbox[1]), (self.bbox[2],self.bbox[3]), 0xFF, -1)
rgb_msked = np.zeros_like(self.rgb)
rgb_msked[msk==0xFF, :] = self.rgb[msk==0xFF, :]
# construct new imgmsg
msg = Image()
msg.header = self.imgmsg.header
msg.height, msg.width, msg.step = self.imgmsg.height, self.imgmsg.width, self.imgmsg.step
msg.encoding = self.imgmsg.encoding
msg.is_bigendian = self.imgmsg.is_bigendian
msg.data = rgb_msked.tobytes()
self.rgb2_pub.publish(msg)
self.rate.sleep()
# release GPU memory for next data
del self.img
def appendMessages(self, stamp, messages):
if not self._image is None:
# Build the image message and push it on the message list
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = self._frameId
msg.width = self._image.shape[0]
msg.height = self._image.shape[1]
if (len(self._image.shape) == 2) or (len(self._image.shape) == 3 and self._image.shape[2] == 1):
# A gray image
msg.encoding = '8UC1'
stepMult = 1
elif len(self._image.shape) == 3 and self._image.shape[2] == 3:
# A color image
msg.encoding = 'rgb8'
stepMult = 3
elif len(self._image.shape) == 3 and self._image.shape[2] == 4:
# A color image
msg.encoding = 'rgba8'
stepMult = 3
else:
raise RuntimeError("The parsing of images is very simple. " +\
"Only 3-channel rgb (rgb8), 4 channel rgba " +\
"(rgba8) and 1 channel mono (mono8) are " +\
"supported. Got an image with shape " +\
"{0}".format(self._image.shape))
msg.is_bigendian = False
msg.step = stepMult * msg.width
msg.data = self._image.flatten().tolist()
messages.append((self._topic, msg))
def convert(data): width=data.info.width height=data.info.height pixelList = [0] *width*height imageMsg=Image() imageMsg.header.stamp = rospy.Time.now() imageMsg.header.frame_id = '1' imageMsg.height = height imageMsg.width = width imageMsg.encoding = 'mono8' imageMsg.is_bigendian = 0 imageMsg.step = width for h in range(height): for w in range(width): if data.data[h*width+w]==-1: pixelList[h*width+w] = 150 elif data.data[h*width+w]==0: pixelList[h*width+w] = 0 elif data.data[h*width+w]==100: pixelList[h*width+w] = 255 else: pixelList[h*width+w]=data.data[h*width+w] print 'ERROR' imageMsg.data = pixelList imagePub.publish(imageMsg)
def step(self):
stamp = super().step()
if not stamp:
return
# Publish camera data
if self._image_publisher.get_subscription_count(
) > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
# Image data
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.height = self._wb_device.getHeight()
msg.width = self._wb_device.getWidth()
msg.is_bigendian = False
msg.step = self._wb_device.getWidth() * 4
if self._wb_device.getRangeImage() is None:
return
image_array = np.array(self._wb_device.getRangeImage(),
dtype="float32")
# We pass `data` directly to we avoid using `data` setter.
# Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally.
# Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
# behavior.
# deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
msg._data = image_array.tobytes()
msg.encoding = '32FC1'
self._image_publisher.publish(msg)
else:
self._wb_device.disable()
def step(self):
stamp = super().step()
if not stamp:
return
# Publish camera data
if self._image_publisher.get_subscription_count(
) > 0 or self._always_publish:
self._wb_device.enable(self._timestep)
image = self._wb_device.getImage()
if image is None:
return
# Image data
msg = Image()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
msg.height = self._wb_device.getHeight()
msg.width = self._wb_device.getWidth()
msg.is_bigendian = False
msg.step = self._wb_device.getWidth() * 4
# We pass `data` directly to we avoid using `data` setter.
# Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally.
# Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable
# behavior.
# deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`.
msg._data = image
msg.encoding = 'bgra8'
self._image_publisher.publish(msg)
self.__message_info.header.stamp = Time(
seconds=self._node.robot.getTime()).to_msg()
self._camera_info_publisher.publish(self.__message_info)
if self._wb_device.hasRecognition(
) and self._recognition_publisher.get_subscription_count() > 0:
self._wb_device.recognitionEnable(self._timestep)
objects = self._wb_device.getRecognitionObjects()
if objects is None:
return
# Recognition data
msg = RecognitionObjects()
msg.header.stamp = stamp
msg.header.frame_id = self._frame_id
for obj in objects:
msg_obj = RecognitionObject()
msg_obj.position = obj.get_position()
msg_obj.position_on_image = obj.get_position_on_image()
msg_obj.size_on_image = obj.get_size_on_image()
msg_obj.number_of_colors = obj.get_number_of_colors()
msg_obj.colors = obj.get_colors()
msg_obj.model = str(obj.get_model())
msg.objects.append(msg_obj)
self._recognition_publisher.publish(msg)
else:
self._wb_device.recognitionDisable()
else:
self._wb_device.disable()
def record_rgb():
"""Recording 2D visuals
"""
# Get the RGB image from the Kinect sensor
# Note: There are issues with wait_for_message not getting the
# most up to date message from /camera/rgb/image_raw.
# This is a hack to wait for the updated message
current_time = rospy.get_rostime()
orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image)
while orig_rgb.header.stamp < current_time:
orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image)
# Copy the recevied Image message to a new Image message
copy_rgb = Image()
copy_rgb.header = orig_rgb.header
copy_rgb.height = orig_rgb.height
copy_rgb.width = orig_rgb.width
copy_rgb.encoding = orig_rgb.encoding
copy_rgb.is_bigendian = orig_rgb.is_bigendian
copy_rgb.step = orig_rgb.step
copy_rgb.data = orig_rgb.data
# Publish the new Image to be saved with the specific name
rgb_pub = rospy.Publisher('rgb_image', Image, queue_size=1)
rospy.loginfo('-------------------------------------------------')
rospy.loginfo('Generating RGB')
rospy.loginfo('-------------------------------------------------')
rospy.sleep(3)
rgb_pub.publish(copy_rgb)
def timer_callback(self):
"""Timer Callback Function
This method captures images and publishes required data in ros 2 topic.
"""
if self.capture.isOpened():
# reads image data
ret, frame = self.capture.read()
# processes image data and converts to ros 2 message
msg = Image()
msg.header.stamp = Node.get_clock(self).now().to_msg()
msg.header.frame_id = 'ANI717'
msg.height = np.shape(frame)[0]
msg.width = np.shape(frame)[1]
msg.encoding = "bgr8"
msg.is_bigendian = False
msg.step = np.shape(frame)[2] * np.shape(frame)[1]
msg.data = np.array(frame).tobytes()
# publishes message
self.publisher_.publish(msg)
self.get_logger().info('%d Images Published' % self.i)
# image counter increment
self.i += 1
return None
def applyDepthMask(self, image_msg, mask_msg, blur):
# height and width
height = image_msg.height
width = image_msg.width
# get images
image_raw = np.fromstring(image_msg.data, np.uint8)
image_blur = np.full((height * width * 3, 1), 225)
mask = np.fromstring(mask_msg.data, np.uint8)
# reshape arrrays (fromstring) to matrices
mask = mask.reshape(height * width, 1)
image_raw = image_raw.reshape(height * width * 3, 1)
# for index i in images,
# select image_raw[i] when mask[i] true
# select image_blur[i] when mask[i] false
image_masked = np.empty(image_raw.shape)
for i in range(3):
image_masked[i::3] = np.where(mask, image_raw[i::3], image_blur[i::3])
# create sensor_msgs image
image_msg_out = Image()
image_msg_out.header.stamp = image_msg.header.stamp
image_msg_out.header.frame_id = image_msg.header.frame_id
image_msg_out.height = image_msg.height
image_msg_out.width = image_msg.width
image_msg_out.encoding = image_msg.encoding
image_msg_out.is_bigendian = image_msg.is_bigendian
image_msg_out.step = image_msg.step
image_msg_out.data = list(image_masked)
# result
return image_msg_out
def publishImage(self, type="color"):
responses = self.client.simGetImages([
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)
]) #scene vision image in uncompressed RGB array
for response in responses:
img_rgb_string = response.image_data_uint8
# Populate image message
msg = Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "frameId"
msg.encoding = "rgb8"
msg.height = 360 # resolution should match values in settings.json
msg.width = 640
msg.data = img_rgb_string
msg.is_bigendian = 0
msg.step = msg.width * 4
# log time and size of published image
rospy.loginfo(len(response.image_data_uint8))
# publish image message
# msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring()
self.image_pub.publish(msg)
# sleep until next cycle
rospy.Rate(10).sleep()
def camera_callback(self):
# Image data
msg = Image()
msg.height = self.camera.getHeight()
msg.width = self.camera.getWidth()
msg.is_bigendian = False
msg.step = self.camera.getWidth() * 4
msg.data = self.camera.getImage()
msg.encoding = 'bgra8'
self.camera_publisher.publish(msg)
# CameraInfo data
msg = CameraInfo()
msg.header.frame_id = 'camera_frame'
msg.height = self.camera.getHeight()
msg.width = self.camera.getWidth()
msg.distortion_model = 'plumb_bob'
msg.d = [0.0, 0.0, 0.0, 0.0, 0.0]
msg.k = [
self.camera.getFocalLength(), 0.0, self.camera.getWidth() / 2,
0.0, self.camera.getFocalLength(), self.camera.getHeight() / 2,
0.0, 0.0, 1.0
]
msg.p = [
self.camera.getFocalLength(), 0.0, self.camera.getWidth() / 2, 0.0,
0.0, self.camera.getFocalLength(), self.camera.getHeight() / 2, 0.0,
0.0, 0.0, 1.0, 0.0
]
self.camera_info_publisher.publish(msg)
def cv2_to_imgmsg(cvim, encoding="passthrough"):
"""
Convert an OpenCV :cpp:type:`cv::Mat` type to a ROS sensor_msgs::Image message.
:param cvim: An OpenCV :cpp:type:`cv::Mat`
:param encoding: The encoding of the image data, one of the following strings:
* ``"passthrough"``
* one of the standard strings in sensor_msgs/image_encodings.h
:rtype: A sensor_msgs.msg.Image message
:raises CvBridgeError: when the ``cvim`` has a type that is incompatible with ``encoding``
If encoding is ``"passthrough"``, then the message has the same encoding as the image's OpenCV type.
Otherwise desired_encoding must be one of the standard image encodings
This function returns a sensor_msgs::Image message on success, or raises :exc:`cv_bridge.CvBridgeError` on failure.
"""
import numpy as np
if not isinstance(cvim, (np.ndarray, np.generic)):
raise TypeError('Your input type is not a numpy array')
img_msg = Image()
img_msg.height = cvim.shape[0]
img_msg.width = cvim.shape[1]
cv_type = '8UC3'
if encoding == "passthrough":
img_msg.encoding = cv_type
else:
img_msg.encoding = encoding
if cvim.dtype.byteorder == '>':
img_msg.is_bigendian = True
img_msg.data = cvim.tostring()
img_msg.step = len(img_msg.data) // img_msg.height
return img_msg
def publish( self ):
# Get the image.
image = self.__videoDeviceProxy.getImageRemote( self.__videoDeviceProxyName );
# Create Image message.
ImageMessage = Image();
ImageMessage.header.stamp.secs = image[ 5 ];
ImageMessage.width = image[ 0 ];
ImageMessage.height = image[ 1 ];
ImageMessage.step = image[ 2 ] * image[ 0 ];
ImageMessage.is_bigendian = False;
ImageMessage.encoding = 'bgr8';
ImageMessage.data = image[ 6 ];
self.__imagePublisher.publish( ImageMessage );
# Create CameraInfo message.
# Data from the calibration phase is hard coded for now.
CameraInfoMessage = CameraInfo();
CameraInfoMessage.header.stamp.secs = image[ 5 ];
CameraInfoMessage.width = image[ 0 ];
CameraInfoMessage.height = image[ 1 ];
CameraInfoMessage.D = [ -0.0769218451517258, 0.16183180613612602, 0.0011626049774280595, 0.0018733894100460534, 0.0 ];
CameraInfoMessage.K = [ 581.090096189648, 0.0, 341.0926325830606, 0.0, 583.0323248080421, 241.02441593704128, 0.0, 0.0, 1.0 ];
CameraInfoMessage.R = [ 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 ];
CameraInfoMessage.P = [ 580.5918359588198, 0.0, 340.76398441334106, 0.0, 0.0, 582.4042541534321, 241.04182225157172, 0.0, 0.0, 0.0, 1.0, 0.0] ;
CameraInfoMessage.distortion_model = 'plumb_bob';
#CameraInfoMessage.roi.x_offset = self.__ROIXOffset;
#CameraInfoMessage.roi.y_offset = self.__ROIYOffset;
#CameraInfoMessage.roi.width = self.__ROIWidth;
#CameraInfoMessage.roi.height = self.__ROIHeight;
#CameraInfoMessage.roi.do_rectify = self.__ROIDoRectify;
self.__cameraInfoPublisher.publish( CameraInfoMessage );
def airpub():
pub = rospy.Publisher("airsim/image_raw", Image, queue_size=1)
rospy.init_node('image_raw', anonymous=True)
rate = rospy.Rate(10) # 10hz
# connect to the AirSim simulator
client = airsim.MultirotorClient()
client.confirmConnection()
while not rospy.is_shutdown():
# get camera images from the car
responses = client.simGetImages([
airsim.ImageRequest("1", airsim.ImageType.Scene, False, False)]) #scene vision image in uncompressed RGBA array
for response in responses:
img_rgba_string = response.image_data_uint8
# Populate image message
msg=Image()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "frameId"
msg.encoding = "rgba8"
msg.height = 360 # resolution should match values in settings.json
msg.width = 640
msg.data = img_rgba_string
msg.is_bigendian = 0
msg.step = msg.width * 4
# log time and size of published image
rospy.loginfo(len(response.image_data_uint8))
# publish image message
pub.publish(msg)
# sleep until next cycle
rate.sleep()
def main():
rospy.init_node('capturing', anonymous=True)
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
height = 480
width = 640
camera.resolution = (width, height)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(width, height))
# allow the camera to warmup
time.sleep(0.1)
image_pub = rospy.Publisher('image', Image, queue_size=1)
image_msg = Image()
rate = rospy.Rate(10)
# capture frames from the camera
for frame in camera.capture_continuous(rawCapture,
format="bgr",
use_video_port=True):
image = frame.array
image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
image_msg.header.stamp = rospy.get_rostime()
image_msg.header.frame_id = 'raspicam'
image_msg.encoding = 'rgb8'
image_msg.height = height
image_msg.width = width
image_msg.step = width * 3
image_msg.data = image.tostring()
image_msg.is_bigendian = True
image_pub.publish(image_msg)
rawCapture.truncate(0)
rate.sleep()
def numpy_to_image(arr, encoding):
if not encoding in name_to_dtypes:
raise TypeError('Unrecognized encoding {}'.format(encoding))
im = Image(encoding=encoding)
# extract width, height, and channels
dtype_class, exp_channels = name_to_dtypes[encoding]
dtype = np.dtype(dtype_class)
if len(arr.shape) == 2:
im.height, im.width, channels = arr.shape + (1, )
elif len(arr.shape) == 3:
im.height, im.width, channels = arr.shape
else:
raise TypeError("Array must be two or three dimensional")
# check type and channels
if exp_channels != channels:
raise TypeError("Array has {} channels, {} requires {}".format(
channels, encoding, exp_channels))
if dtype_class != arr.dtype.type:
raise TypeError("Array is {}, {} requires {}".format(
arr.dtype.type, encoding, dtype_class))
# make the array contiguous in memory, as mostly required by the format
contig = np.ascontiguousarray(arr)
im.data = contig.tostring()
im.step = contig.strides[0]
im.is_bigendian = (arr.dtype.byteorder == '>' or arr.dtype.byteorder == '='
and sys.byteorder == 'big')
return im
def copy_Image_empty_data(image_old):
image_new = Image()
image_new.header = copy(image_old.header)
image_new.height = copy(image_old.height)
image_new.width = copy(image_old.width)
image_new.encoding = copy(image_old.encoding)
image_new.is_bigendian = copy(image_old.is_bigendian)
image_new.step = copy(image_old.step)
return image_new
def publishCombined(self):
#Enter Main Loop
while not rospy.is_shutdown():
#Convert to Numpy Arrays
map = []
for i in range(0, self.numRobots):
map.append(numpy.array(self.searchedData[i].data))
combined2 = map[0]
if self.numRobots > 1:
#Find Minimum of all maps
for i in range(1, self.numRobots):
combined2 = numpy.minimum(combined2, map[i])
#Pack Occupancy Grid Message
mapMsg = OccupancyGrid()
mapMsg.header.stamp = rospy.Time.now()
mapMsg.header.frame_id = self.mapData.header.frame_id
mapMsg.info.resolution = self.mapData.info.resolution
mapMsg.info.width = self.mapData.info.width
mapMsg.info.height = self.mapData.info.height
mapMsg.info.origin = self.mapData.info.origin
mapMsg.data = combined2.tolist()
#Convert combined Occupancy grid values to grayscal image values
combined2[combined2 == -1] = 150 #Unknown -1->150 (gray)
combined2[combined2 == 100] = 255 #Not_Searched 100->255 (white)
#Searched=0 (black)
#Calculate percentage of open area searched
numNotSearched = combined2[combined2 == 255].size
numSearched = combined2[combined2 == 0].size
percentSearched = 100 * float(numSearched) / (numNotSearched +
numSearched)
percentSearchedMsg = Float32()
percentSearchedMsg.data = percentSearched
self.percentPub.publish(percentSearchedMsg)
#Pack Image Message
imageMsg = Image()
imageMsg.header.stamp = rospy.Time.now()
imageMsg.header.frame_id = self.mapData.header.frame_id
imageMsg.height = self.mapData.info.height
imageMsg.width = self.mapData.info.width
imageMsg.encoding = 'mono8'
imageMsg.is_bigendian = 0
imageMsg.step = self.mapData.info.width
imageMsg.data = combined2.tolist()
#Publish Combined Occupancy Grid and Image
self.searchedCombinePub.publish(mapMsg)
self.imagePub.publish(imageMsg)
#Update Every 0.5 seconds
rospy.sleep(1.0)
def record_rgb(object_name):
"""Recording 2D visuals
Spawn the object, record the visuals and delete the object
Parameters
----------
object_name : str
The name of the tool or object to be recorded
"""
# Spawn the object
rospack = rospkg.RosPack()
object_urdf_path = str(
rospack.get_path('affordance_experiment') + '/models/' + object_name +
'.urdf')
pitch = pi / 2
yaw = 0
if object_name == 'cylinder':
yaw = pi / 2
spawn_object(object_name, object_urdf_path, 0.0, 0.35, 1.05, pi / 2, pitch,
yaw)
# Wait for the object to rest
rospy.sleep(2)
# Get the RGB image from the Kinect sensor
# Note: There are issues with wait_for_message not getting the
# most up to date message from /camera/rgb/image_raw.
# This is a hack to wait for the updated message
current_time = rospy.get_rostime()
orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image)
while orig_rgb.header.stamp < current_time:
orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image)
# Copy the recevied Image message to a new Image message
copy_rgb = Image()
copy_rgb.header = orig_rgb.header
copy_rgb.height = orig_rgb.height
copy_rgb.width = orig_rgb.width
copy_rgb.encoding = orig_rgb.encoding
copy_rgb.is_bigendian = orig_rgb.is_bigendian
copy_rgb.step = orig_rgb.step
copy_rgb.data = orig_rgb.data
# Publish the new Image to be saved with the specific name
rgb_pub = rospy.Publisher(object_name + '_rgb', Image, queue_size=1)
rospy.loginfo('-------------------------------------------------')
rospy.loginfo('Generating RGB')
rospy.loginfo('-------------------------------------------------')
rospy.sleep(3)
rgb_pub.publish(copy_rgb)
# Delete object
delete_object(object_name)
rospy.sleep(3)
def construct_image(header: Header, response: ImageResponse, encoding: str) -> Image:
msg = Image()
msg.header = header
msg.encoding = encoding
msg.height = response.height
msg.width = response.width
msg.data = response.image_data_uint8 if response.image_type != ImageType.DepthPlanar else response.image_data_float
msg.is_bigendian = 0
msg.step = response.width * 3
return msg
def publishCombined(self): #Enter Main Loop while not rospy.is_shutdown(): #Convert to Numpy Arrays map = [] for i in range(0, self.numRobots): map.append(numpy.array(self.searchedData[i].data)) combined2 = map[0] if self.numRobots > 1: #Find Minimum of all maps for i in range(1, self.numRobots): combined2 = numpy.minimum(combined2,map[i]) #Pack Occupancy Grid Message mapMsg=OccupancyGrid() mapMsg.header.stamp=rospy.Time.now() mapMsg.header.frame_id=self.mapData.header.frame_id mapMsg.info.resolution=self.mapData.info.resolution mapMsg.info.width=self.mapData.info.width mapMsg.info.height=self.mapData.info.height mapMsg.info.origin=self.mapData.info.origin mapMsg.data=combined2.tolist() #Convert combined Occupancy grid values to grayscal image values combined2[combined2 == -1] = 150 #Unknown -1->150 (gray) combined2[combined2 == 100] = 255 #Not_Searched 100->255 (white) #Searched=0 (black) #Calculate percentage of open area searched numNotSearched = combined2[combined2==255].size numSearched = combined2[combined2==0].size percentSearched = 100*float(numSearched)/(numNotSearched+numSearched) percentSearchedMsg = Float32() percentSearchedMsg.data = percentSearched self.percentPub.publish(percentSearchedMsg) #Pack Image Message imageMsg=Image() imageMsg.header.stamp = rospy.Time.now() imageMsg.header.frame_id = self.mapData.header.frame_id imageMsg.height = self.mapData.info.height imageMsg.width = self.mapData.info.width imageMsg.encoding = 'mono8' imageMsg.is_bigendian = 0 imageMsg.step = self.mapData.info.width imageMsg.data = combined2.tolist() #Publish Combined Occupancy Grid and Image self.searchedCombinePub.publish(mapMsg) self.imagePub.publish(imageMsg) #Update Every 0.5 seconds rospy.sleep(1.0)
def imageCallback(self, msg):
'''!
Recebe a imagem, seg,emta e publica
'''
img = np.array(msg.data, dtype=np.uint8)
img = np.array(np.split(img, msg.height))
img = np.array(np.split(img, msg.width, axis=1))
img = np.rot90(img)
img = tf.keras.preprocessing.image.array_to_img(img)
image_array = tf.keras.preprocessing.image.img_to_array(img)
result = self.bodypix_model.predict_single(image_array)
mask = result.get_mask(threshold=0.75)
colored_mask = result.get_colored_part_mask(mask)
mask = mask.numpy().astype(np.uint8)
colored_mask = colored_mask.astype(np.uint8)
maskMsg = Image()
maskMsg._data = mask.flatten().tolist()
maskMsg.height = mask.shape[0]
maskMsg.width = mask.shape[1]
maskMsg.encoding = "8UC1"
maskMsg.is_bigendian = 0
maskMsg.step = maskMsg.width
colMsg = Image()
colMsg._data = colored_mask.flatten().tolist()
colMsg.height = colored_mask.shape[0]
colMsg.width = colored_mask.shape[1]
colMsg.encoding = "8UC3"
colMsg.is_bigendian = 0
colMsg.step = colMsg.width * 3
maskMsg.header = msg.header
colMsg.header = msg.header
self.pubMask.publish(maskMsg)
self.colMask.publish(colMsg)
def genRosDepthImage(resolution):
rosImg = Image()
# rosImg.data = np_img.flatten().tobytes()
rosImg.header.frame_id = "rgbd_camera_link"
# rosImg.header.stamp = time_now
rosImg.width = resolution[0] # 宽
rosImg.height = resolution[1] # 高
rosImg.step = resolution[0] * 4
rosImg.encoding = "32FC1"
rosImg.is_bigendian = 0
return rosImg
def decode_image(self, frame, orig_msg):
msg = Image()
msg.data = frame.to_rgb().planes[0].to_bytes()
msg.width = frame.width
msg.height = frame.height
msg.step = frame.width * 3
msg.is_bigendian = 0
msg.encoding = 'rgb8'
msg.header = Header()
msg.header = orig_msg.header
self.pub.publish(msg)
def cv2_to_imgmsg(cv_image):
img_msg = Image()
img_msg.height = cv_image.shape[0]
img_msg.width = cv_image.shape[1]
img_msg.encoding = "bgr8"
img_msg.is_bigendian = 0
img_msg.data = cv_image.tostring()
img_msg.step = len(
img_msg.data
) // img_msg.height # That double line is actually integer division, not a comment
return img_msg
def _publish_img(self, obs):
# Hardcoded Implementation of ros_numpy's ImageConverter
img_msg = Image(encoding='uint8')
img_msg.height, img_msg.width, _ = obs.shape
contig = np.ascontiguousarray(obs)
img_msg.data = contig.tostring()
img_msg.step = contig.strides[0]
img_msg.is_bigendian = (obs.dtype.byteorder == '>'
or obs.dtype.byteorder == '='
and sys.byteorder == 'big')
self.cam_pub.publish(img_msg)
def CreateDepthMessage(img_depth, img_time, sequence):
msg_d = Image()
bridge_d = CvBridge()
msg_d.header.stamp = img_time
msg_d.header.seq = sequence
msg_d.header.frame_id = "world"
msg_d.encoding = "32FC1"
msg_d.height = img_depth.shape[0]
msg_d.width = img_depth.shape[1]
msg_d.data = bridge_d.cv2_to_imgmsg(img_depth, "32FC1").data
msg_d.is_bigendian = 0
msg_d.step = msg_d.width * 4
return msg_d
def CreateRGBMessage(img_rgb, img_time, sequence):
msg_rgb = Image()
bridge_rgb = CvBridge()
msg_rgb.header.stamp = img_time
msg_rgb.header.seq = sequence
msg_rgb.header.frame_id = "world"
msg_rgb.encoding = "bgr8"
msg_rgb.height = img_rgb.shape[0]
msg_rgb.width = img_rgb.shape[1]
msg_rgb.data = bridge_rgb.cv2_to_imgmsg(img_rgb, "bgr8").data
msg_rgb.is_bigendian = 0
msg_rgb.step = msg_rgb.width * 3
return msg_rgb
def callback(self, data):
#print "got some shit coming in"
#if data is not None:
#plane= data.pose.position
#nrm= norm([plane.x, plane.y, plane.z])
#normal= np.array([plane.x, plane.y, plane.z])/nrm
#print "got here"
##replace with numpy array
#plane= np.array([plane.x, plane.y, plane.z])
##get the rotation matrix
#rmatrix= rotationMatrix(normal)
##print rmatrix
##for point in data.points:
##print point
##p= np.array([point.x, point.y, point.z])
##flattened_point= project(p, plane, normal)
##print flattened_point
##print np.dot(rmatrix,flattened_point)
try:
resp= self.seperation()
print resp.result
#self.pub.publish(resp.clusters[0])
im= Image()
im.header.seq= 72
im.header.stamp.secs= 1365037570
im.header.stamp.nsecs= 34077284
im.header.frame_id= '/camera_rgb_optical_frame'
im.height= 480
im.width= 640
im.encoding= '16UC1'
im.is_bigendian= 0
im.step= 1280
im.data= [100 for n in xrange(1280*480)]
for point in resp.clusters[0].points:
x= point.x * 640
y= point.y * 480
im.data[y*1280 + x] = 10
pub_image.publish(im)
except Exception, e:
print "service call failed"
print e
def CreateMonoMessage(img_rgb, img_time, sequence):
msg_mono = Image()
bridge_mono = CvBridge()
img_mono = cv2.cvtColor(img_rgb, cv2.COLOR_BGR2GRAY)
msg_mono.header.stamp = img_time
msg_mono.header.seq = sequence
msg_mono.header.frame_id = "world"
msg_mono.encoding = "mono8"
msg_mono.height = img_rgb.shape[0]
msg_mono.width = img_rgb.shape[1]
msg_mono.data = bridge_mono.cv2_to_imgmsg(img_mono, "mono8").data
msg_mono.is_bigendian = 0
msg_mono.step = msg_mono.width
return msg_mono
def map_image(values):
"""
Map the values generated with the hypothesis-ros image strategy to a rospy Image type.
"""
if not isinstance(values, _Image):
raise TypeError('Wrong type. Use appropriate hypothesis-ros type.')
ros_image = Image()
ros_image.header = values.header
ros_image.height = values.height
ros_image.width = values.width
ros_image.encoding = values.encoding
ros_image.is_bigendian = values.is_bigendian
ros_image.data = values.data
return ros_image
def _array_to_imgmsg(img_array, encoding):
assert len(img_array.shape) == 3
img_msg = Image()
img_msg.height = img_array.shape[0]
img_msg.width = img_array.shape[1]
if encoding == 'passthrough':
img_msg.encoding = '8UC3'
else:
img_msg.encoding = encoding
if img_array.dtype.byteorder == '>':
img_msg.is_bigendian = True
img_msg.data = img_array.tostring()
img_msg.step = len(img_msg.data) // img_msg.height
return img_msg
def main():
global raw_image_msg
rospy.init_node('object_detection')
rospy.Subscriber("/raw_image", Image, object_detection_callback)
p = pr.Predictor()
image_pub = rospy.Publisher("/image", Image, queue_size=10)
while raw_image_msg is None:
continue
azure_addr = "137.135.81.74"
while not rospy.is_shutdown():
image_array = list_to_array(raw_image_msg.data, raw_image_msg.height,
raw_image_msg.width)
try:
cv2.imwrite("./input.png", image_array)
# upload to azure
os.system("scp -r ./input.png azureuser@{}:/home/azureuser".format(
azure_addr))
# download from azure
os.system("scp azureuser@{}:/home/azureuser/output.png ./".format(
azure_addr))
output = cv2.imread("output.png")
except Exception as e:
print(e)
print(
"Something went wrong when communicating with azure, trying again"
)
continue
# print(image_array)
# output = p.transform(image_array)
# print(numpy.all(output == 0))
rgb_list, h, w = array_to_list(output)
# print(rgb_list)
image_msg = Image()
image_msg.header.stamp = rospy.Time.now()
image_msg.header.frame_id = 'a'
image_msg.height = h
image_msg.width = w
image_msg.encoding = 'bgr8'
image_msg.is_bigendian = 1
image_msg.step = 3 * w
image_msg.data = rgb_list
image_pub.publish(image_msg)
def publish_image(self):
# get the image from the Nao
img = self.nao_cam.getImageRemote(self.proxy_name)
# copy the data into the ROS Image
ros_img = Image()
ros_img.width = img[0]
ros_img.height = img[1]
ros_img.step = img[2] * img[0]
ros_img.header.stamp.secs = img[5]
ros_img.data = img[6]
ros_img.is_bigendian = False
ros_img.encoding = "rgb8"
ros_img.data = img[6]
# publish the image
self.nao_cam_pub.publish(ros_img)
def OccupancyGridToNavImage(self, grid_map):
img = Image()
img.header = grid_map.header
img.height = grid_map.info.height
img.width = grid_map.info.width
img.is_bigendian = 1
img.step = grid_map.info.width
img.encoding = "mono8"
maxindex = img.height*img.width
numpy.uint
for i in range(0, maxindex, 1):
if int(grid_map.data[i]) < 20:
#img.data[i] = "0"
data.append(0)
else:
img.data[i] = "255"
return imgding
def array_to_image(array):
"""Takes a NxMx3 array and converts it into a ROS image message.
"""
# Sanity check the input array shape
if len(array.shape) != 3 or array.shape[2] != 3:
raise ValueError('Array must have shape MxNx3')
# Ravel the array into a single buffer
image_data = (array.astype(np.uint8)).tostring(order='C')
# Create the image message
image_msg = Image()
image_msg.height = array.shape[0]
image_msg.width = array.shape[1]
image_msg.encoding = 'rgb8'
image_msg.is_bigendian = 0
image_msg.step = array.shape[1] * 3
image_msg.data = image_data
return image_msg
def main():
pub = rospy.Publisher('image_maker', Image)
rospy.init_node('image_maker')
#rospy.wait_for_service('tabletop_segmentation')
im= Image()
im.header.seq= 72
im.header.stamp.secs= 1365037570
im.header.stamp.nsecs= 34077284
im.header.frame_id= '/camera_rgb_optical_frame'
im.height= 480
im.width= 640
im.encoding= '16UC1'
im.is_bigendian= 0
im.step= 1280
im.data= [100 for n in xrange(1280*480)]
while not rospy.is_shutdown():
try:
pub.publish(im)
sleep(.5)
except Exception, e:
print e
def numpy_to_image(arr, encoding):
if not encoding in name_to_dtypes:
raise TypeError('Unrecognized encoding {}'.format(encoding))
im = Image(encoding=encoding)
# extract width, height, and channels
dtype_class, exp_channels = name_to_dtypes[encoding]
dtype = np.dtype(dtype_class)
if len(arr.shape) == 2:
im.height, im.width, channels = arr.shape + (1,)
elif len(arr.shape) == 3:
im.height, im.width, channels = arr.shape
else:
raise TypeError("Array must be two or three dimensional")
# check type and channels
if exp_channels != channels:
raise TypeError("Array has {} channels, {} requires {}".format(
channels, encoding, exp_channels
))
if dtype_class != arr.dtype.type:
raise TypeError("Array is {}, {} requires {}".format(
arr.dtype.type, encoding, dtype_class
))
# make the array contiguous in memory, as mostly required by the format
contig = np.ascontiguousarray(arr)
im.data = contig.tostring()
im.step = contig.strides[0]
im.is_bigendian = (
arr.dtype.byteorder == '>' or
arr.dtype.byteorder == '=' and sys.byteorder == 'big'
)
return im
def find_object(self):
with self.lock_depth:
image_data = self.image_data
depth_data = self.depth_data
pub_data = rospy.Publisher('/racecar/image/data', Float64MultiArray, queue_size=1)
arrayData = Float64MultiArray()
#print "received image"
if image_data:
newImage = Image()
newImage.encoding = image_data.encoding
newImage.header = image_data.header
newImage.is_bigendian = image_data.is_bigendian
newData = bytearray()
image = image_data.data
clusters = []
white = chr(255) *3
black = chr(0) * 3
red = chr(0) + chr(0) + chr(255)
blue = chr(255) + chr(0) * 2
#defining color search thresholds
redThreshold = 230
blueThreshold = 20
greenLowerThreshold = 50
greenUpperThreshold = 200
height = 0
starttime = time.time()
for h in xrange(0, image_data.height, self.resolution):
height += 1
for w in xrange(0, image_data.width, self.resolution):
index = w * 3 + image_data.width * 3 * h
b = ord(image[index])
g = ord(image[index + 1])
r = ord(image[index + 2])
#filtering for the color
if r > redThreshold and b < blueThreshold and g <= greenUpperThreshold and g >= greenLowerThreshold:
for char in white:
newData.append(char)
inCluster = False
for cluster in clusters:
if self.checkAdjacency((w, h), cluster):
cluster.add((w, h))
inCluster = True
if not inCluster:
newCluster = set()
newCluster.add((w, h))
clusters.append(newCluster)
else:
for char in black:
newData.append(char)
newImage.height = height
newImage.width = len(newData)/height/3
newImage.step = len(newData)/height
newImage.data = str(newData)
#print 'time taken: ', time.time() - starttime
clusters = self.reduceClusters(clusters)
#print len(clusters), float(len(candidates_x))/len(newData)
#print [len(cluster) for cluster in clusters]
if len(clusters) > 1:
print "multiple objects found"
print [len(cluster) for cluster in clusters]
if len(clusters):
publishableClusters = [max(clusters, key=lambda x:len(x))]
data_to_publish = []
for cluster in publishableClusters:
centerX = sum(x for (x, y) in cluster)/len(cluster)
centerY = sum(y for (x, y) in cluster)/len(cluster)
width = max(cluster, key=lambda (x, y):x)[0] - min(cluster, key=lambda (x, y):x)[0]
height = max(cluster, key=lambda (x, y):y)[1] - min(cluster, key=lambda (x, y):y)[1]
relativeLocation = float(centerX)/image_data.width - 0.5
relativeAngle = relativeLocation * 100 * math.pi / 180
data_to_publish += [centerX, centerY, width, height, relativeAngle]
#print 'center ', centerX, centerY, len(candidates_x)
#print 'distance ', depth1, depth2
#print 'dim ', height, width
'''depth_index = centerX * 2 + image_data.width * 2 * centerY
depth1 = ord(depth_data.data[depth_index])
depth2 = ord(depth_data.data[depth_index + 1])'''
arrayData.data = data_to_publish
else:
arrayData.data = [-1, -1, -1, -1, -1]
print 'no cone found'
pub_data.publish(arrayData)
#print "publishing"
pub_img = rospy.Publisher('/racecar/image/newImage', Image, queue_size=1)
pub_img.publish(newImage)
else:
arrayData.data = [-1, -1, -1, -1]
print 'image not ready'
def spin(self):
while not rospy.is_shutdown():
m = MarkerArray()
stamp, f1_tact, f2_tact, f3_tact, palm_tact = self.bh.getTactileData()
finger_skin_data = []
finger_skin_data.append(f1_tact)
finger_skin_data.append(f2_tact)
finger_skin_data.append(f3_tact)
finger_skin_data.append(palm_tact)
frame_id = []
frame_id.append(self.prefix+"_HandFingerOneKnuckleThreeLink")
frame_id.append(self.prefix+"_HandFingerTwoKnuckleThreeLink")
frame_id.append(self.prefix+"_HandFingerThreeKnuckleThreeLink")
frame_id.append(self.prefix+"_HandPalmLink")
arrows = False
if arrows:
for sens in range(0, 4):
for i in range(0, 24):
halfside1 = PyKDL.Vector(pressure_info.sensor[sens].halfside1[i].x, pressure_info.sensor[sens].halfside1[i].y, pressure_info.sensor[sens].halfside1[i].z)
halfside2 = PyKDL.Vector(pressure_info.sensor[sens].halfside2[i].x, pressure_info.sensor[sens].halfside2[i].y, pressure_info.sensor[sens].halfside2[i].z)
# calculate cross product: halfside1 x halfside2
norm = halfside1*halfside2
norm.Normalize()
scale = 0
if sens == 0:
scale = data.finger1_tip[i]/256.0
elif sens == 1:
scale = data.finger2_tip[i]/256.0
elif sens == 2:
scale = data.finger3_tip[i]/256.0
else:
scale = data.palm_tip[i]/256.0
norm = norm*scale*0.01
marker = Marker()
marker.header.frame_id = frame_id[sens]
marker.header.stamp = rospy.Time.now()
marker.ns = frame_id[sens]
marker.id = i
marker.type = 0
marker.action = 0
marker.points.append(Point(pressure_info.sensor[sens].center[i].x,pressure_info.sensor[sens].center[i].y,pressure_info.sensor[sens].center[i].z))
marker.points.append(Point(pressure_info.sensor[sens].center[i].x+norm.x(), pressure_info.sensor[sens].center[i].y+norm.y(), pressure_info.sensor[sens].center[i].z+norm.z()))
marker.pose = Pose( Point(0,0,0), Quaternion(0,0,0,1) )
marker.scale = Vector3(0.001, 0.002, 0)
marker.color = ColorRGBA(1,0,0,1)
m.markers.append(marker)
self.pub.publish(m)
else:
for sens in range(0, 4):
for i in range(0, 24):
value = self.convertToRGB(int(finger_skin_data[sens][i]/2))
marker = Marker()
marker.header.frame_id = frame_id[sens]
marker.header.stamp = rospy.Time.now()
marker.ns = frame_id[sens]
marker.id = i
marker.type = 1
marker.action = 0
if sens < 3:
marker.pose = pm.toMsg(self.bh.pressure_frames[i])
marker.scale = Vector3(self.bh.pressure_cells_size[i][0], self.bh.pressure_cells_size[i][1], 0.004)
else:
marker.pose = pm.toMsg(self.bh.palm_pressure_frames[i])
marker.scale = Vector3(self.bh.palm_pressure_cells_size[i][0], self.bh.palm_pressure_cells_size[i][1], 0.004)
marker.color = ColorRGBA(1.0/255.0*value[0], 1.0/255.0*value[1], 1.0/255.0*value[2],1)
m.markers.append(marker)
self.pub.publish(m)
# palm f1 f2 f3
# xxx xxx xxx
# xxx xxx xxx
# xxxxx xxx xxx xxx
# xxxxxxx xxx xxx xxx
# 56789xx 9xx 9xx 9xx
# 01234 678 678 678
# 345 345 345
# 012 012 012
im = Image()
im.height = 8
im.width = 19
im.encoding = "rgb8"
im.is_bigendian = 0
im.step = im.width*3
im.data = [0]*(im.step*im.height)
for finger in range(0, 3):
for y in range(0, 8):
for x in range(0, 3):
xim = 8 + x + finger * 4
yim = im.height-1-y
value = self.convertToRGB(int(finger_skin_data[finger][y*3+x]/2))
im.data[(yim*im.width + xim)*3+0] = value[0]
im.data[(yim*im.width + xim)*3+1] = value[1]
im.data[(yim*im.width + xim)*3+2] = value[2]
palm_im_x = [1, 2, 3, 4, 5, 0, 1, 2, 3, 4, 5, 6, 0, 1, 2, 3, 4, 5, 6, 1, 2, 3, 4, 5]
palm_im_y = [5, 5, 5, 5, 5, 4, 4, 4, 4, 4, 4, 4, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2, 2, 2]
for i in range(0, 24):
xim = palm_im_x[i]
yim = palm_im_y[i]
value = self.convertToRGB(int(finger_skin_data[3][i]/2))
im.data[(yim*im.width + xim)*3+0] = value[0]
im.data[(yim*im.width + xim)*3+1] = value[1]
im.data[(yim*im.width + xim)*3+2] = value[2]
self.tactileImagepub.publish(im)
rospy.sleep(0.1)
UDK_camera=(240,320) #box(left, upper, right, lower) box_A = (0, 0, UDK_camera[1],UDK_camera[0]) box_B = (UDK_camera[1],0,UDK_camera[1]*2,UDK_camera[0]) box_C = (UDK_camera[1]*2,0,UDK_camera[1]*3,UDK_camera[0]) box_D = (UDK_camera[1]*3,0,UDK_camera[1]*4,UDK_camera[0]) clientsock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) clientsock.connect((host,port)) c_msg = 'OK\r\n' clientsock.sendall(c_msg) #set image info img_A=Image() img_A.encoding='rgb8' img_A.is_bigendian =0 img_A.step =1 img_A.height = UDK_camera[0] img_A.width = UDK_camera[1] img_B=Image() img_B.encoding='rgb8' img_B.is_bigendian =0 img_B.step =1 img_B.height = UDK_camera[0] img_B.width = UDK_camera[1] img_C=Image() img_C.encoding='rgb8' img_C.is_bigendian =0 img_C.step =1
