The instructions for the question are below
Question report 2.7. You are now ready to make a ROS node to perform real-time
extraction of the centroid of the line from the images acquired by the camera of your
robot.
File name: image_segment_node.py
Subscribes to topics:
raspicam_node/image (type sensor_msgs/Image )
Publishes to topics:
• /image/segmented (type sensor_msgs/Image )
• /image/centroid (type geometry_msgs/PointStamped )
Description: This node should import the file image_processing.py to use all the
functions that you have developed so far. For each new image received from the camera,
the node should perform the same operations described in Question provided 2.3, but
without visualization of the results. Instead, the node should:
Publish the segmented image with the line on the topic /image/segmented .
Insert the computed horizontal centroid in the x field of a PointStamped message,
add the current time to the header.stamp field, and then publish the message
on the topic /image/centroid
The code I have for image_segment_node.py is below
#!/usr/bin/env python
"""This is a node to perform real-time extraction of the centroid of the line from images acquired by camera of robot and export data."""
import rospy
import numpy as np
import image_processing as ip
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from geometry_msgs.msg import PointStamped
import cv2
def callback(msg):
global pub1
global pub2
rospy.loginfo("started")
img = CvBridge.imgmsg_to_cv2(msg, 'bgr8')
img = cv2.imread(img)
lb, ub = ip.classifier_parameters()
img_seg = cv2.inRange(img, lb, ub)
x = ip.image_centroid_horizontal(img_seg)
img_seg = ip.image_one_to_three_channels(img_seg)
img_seg = ip.image_line_vertical(img_seg, x)
pub1.publish(img_seg)
point1 = PointStamped()
point1.point = x
point1.header = rospy.Time.now()
pub2.publish(point1)
def main():
global pub1
global pub2
rospy.init_node('image_segment_node')
rospy.Subscriber("/raspicam_node/image", Image, callback=callback, queue_size=1)
pub1 = rospy.Publisher("/image/segmented", Image, queue_size=10)
pub2 = rospy.Publisher("/image/centroid", PointStamped, queue_size=10)
rate = rospy.Rate(10)
# rospy.spin()
if __name__ == '__main__':
try:
main()
finally:
pass
Terminal tells me this
drand21#roslab:~/ros_ws/src/me416_lab/nodes$ rostopic echo /image/centroid
WARNING: topic [/image/centroid] does not appear to be published yet
Can you try the code below, I changed only the structure. There doesn't seem any problem with it, but warning is clearly about "pub2" not publishing.
#!/usr/bin/env python
"""This is a node to perform real-time extraction of the centroid of the line from images acquired by camera of robot and export data."""
import rospy
import numpy as np
import image_processing as ip
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from geometry_msgs.msg import PointStamped
import cv2
class image_segment():
# Class function for creating map relations
def __init__(self):
rospy.init_node('image_segment_node')
self.rate = rospy.Rate(10)
# Publishers
self.pub1 = rospy.Publisher("/image/segmented", Image, queue_size=10)
self.pub2 = rospy.Publisher("/image/centroid", PointStamped, queue_size=10)
# Subscribers
rospy.Subscriber("/raspicam_node/image", Image, callback=self.callback_im, queue_size=1)
def callback_im(self, msg):
rospy.loginfo("started")
img = CvBridge.imgmsg_to_cv2(msg, 'bgr8')
img = cv2.imread(img)
lb, ub = ip.classifier_parameters()
img_seg = cv2.inRange(img, lb, ub)
x = ip.image_centroid_horizontal(img_seg)
img_seg = ip.image_one_to_three_channels(img_seg)
img_seg = ip.image_line_vertical(img_seg, x)
self.pub1.publish(img_seg)
point1 = PointStamped()
point1.point = x
point1.header = rospy.Time.now()
self.pub2.publish(point1)
def main(self):
while not rospy.is_shutdown():
###
self.rate.sleep()
if __name__ == '__main__':
try:
im_class = image_segment()
im_class.main()
finally:
pass
Related
WHAT I'M TRYING TO DO?
I am trying to select the cordinates points on mouse click using cv2.setMouseCallback() function and printing the cordinates of every mouse click. I have put on the condition that the cordinates will be printed only until the counter and counterm is less than equal to 5 for two different functons, namely clickEventCTscanImage(event, x, y, flags, param) and clickEventMRIscanImage(event, x, y, flags, param)respectively.
WHAT I WANT TO ACHIEVE?
I want to achieve that when I have selected the 5 points in totall the specific image should get closed and the second image should get automatically open ups and asks to perform the same function. that means the CT Scan image after achieving 5 points should get closed automatically and MRI image should open up for selecting the points and on selecting the points it should also get closed and then further code should executed
WHAT IS THE PROBLEM?
So, based on what I have described above. When I tries to add "return" in the else condition of clickEventCTscanImage(event, x, y, flags, param) and clickEventMRIscanImage(event, x, y, flags, param) function, the windows doesn't get closed and the code doesn't execute further
also if I add cv2.destroyAllWindows() in the else condition then the CT Scan image gets clos but the MRI image doesn't opens up.
CODE SNIPPET
#importing libraries
import argparse
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.cm as cm
import cv2
import imageio
import scipy.ndimage as ndi
import os
from PIL import Image
global CTscanImageCordinates
CTscanImageCordinates = []
global MRIscanImageCordinates
MRIscanImageCordinates = []
counter = 1
# Define Click Function for CT Scan
def clickEventCTscanImage(event, x, y, flags, param):
global counter
if event == cv2.EVENT_LBUTTONDOWN:
if counter <= 5:
print(x,y)
CTscanImageCordinates.append([x,y])
print("hi")
counter = counter + 1
else:
cv2.destroyAllWindows()
counterm = 1
# Define Click Function for CT Scan
def clickEventMRIscanImage(event, x, y, flags, param):
global counterm
if event == cv2.EVENT_LBUTTONDOWN:
if counterm <= 5:
print(x,y)
MRIscanImageCordinates.append([x,y])
print("hi")
counterm = counterm + 1
else:
cv2.destroyAllWindows()
if __name__ == "__main__":
arg = argparse.ArgumentParser()
arg.add_argument("--pathInCTImage", help="Path to CT Scan Image")
arg.add_argument("--pathInMRIImage", help="Path to MRI Scan Image")
args = arg.parse_args()
CTscanImage = cv2.imread(args.pathInCTImage)
MRIscanImage = cv2.imread(args.pathInMRIImage) #MRI Image will be registered
cv2.imshow('CT Scan Image', CTscanImage)
cv2.setMouseCallback('CT Scan Image', clickEventCTscanImage)
cv2.waitKey()
cv2.imshow('MRI Scan Image', MRIscanImage)
cv2.setMouseCallback('MRI Scan Image', clickEventMRIscanImage)
cv2.waitKey()
Please help me in this. Thanks and Regards
I tried using return statement as well as the cv2.destroyAllWindows() statement for moving out of the image window after selecting the points but none of them worked for me.
Solution
import argparse
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.cm as cm
import cv2
import imageio
import scipy.ndimage as ndi
import os
from PIL import Image
class GetCoordsMRICT:
def __init__(self, CTscanImage, MRIscanImage):
self.counter = 0
self.counterm = 0
self.CTScanCoords = []
self.MRIScanCoords = []
# Running the Functions as follows,
cv2.imshow('CT Scan Image', CTscanImage)
cv2.setMouseCallback('CT Scan Image', self.clickEventCTscanImage)
cv2.waitKey()
cv2.imshow('MRI Scan Image', MRIscanImage)
cv2.setMouseCallback('MRI Scan Image', self.clickEventMRIscanImage)
cv2.waitKey()
def clickEventCTscanImage(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
if self.counter <= 5:
print(x,y)
self.CTScanCoords.append([x,y])
self.counter = self.counter + 1
if self.counter == 5:
print('[+] CT Scan Points Completed.')
cv2.destroyAllWindows()
return 0
# Define Click Function for CT Scan
def clickEventMRIscanImage(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
if self.counterm <= 5:
print(x,y)
self.MRIScanCoords.append([x,y])
self.counterm = self.counterm + 1
if self.counterm == 5:
print('[+] MRI Scan Points Completed.')
cv2.destroyAllWindows()
return 0
if __name__ == "__main__":
arg = argparse.ArgumentParser()
arg.add_argument("--ct", help="Path to CT Scan Image")
arg.add_argument("--mr", help="Path to MRI Scan Image")
args = arg.parse_args()
CTscanImage = cv2.imread(args.ct)
MRIscanImage = cv2.imread(args.mr) #MRI Image will be registered
handler = GetCoordsMRICT(CTscanImage, MRIscanImage)
print(f'[+] CT Scan Codes: {handler.CTScanCoords}')
print(f'[+] MRI Scan Codes: {handler.MRIScanCoords}')
I recommend using classes in python and below is the code to get the co-ordinates. The following class returns the five require co-ordinates as a list.
Output
Refrence
https://docs.python.org/3/tutorial/classes.html
I am working on Udacity's self-driving car simulator. I am facing a problem in this when I run the drive.py file with my model as argument model.h5 nothing happens in the simulator.
The model has been trained completely without any errors but still, there is no response from the simulator.
Here is the drive.py python code and a link to the video to show what is actually happening
drive.py
import argparse
import base64
from datetime import datetime
import os
import shutil
import numpy as np
import socketio
import eventlet
import eventlet.wsgi
from PIL import Image
from flask import Flask
from io import BytesIO
from keras.models import load_model
import h5py
from keras import __version__ as keras_version
sio = socketio.Server()
app = Flask(__name__)
model = None
prev_image_array = None
class SimplePIController:
def __init__(self, Kp, Ki):
self.Kp = Kp
self.Ki = Ki
self.set_point = 0.
self.error = 0.
self.integral = 0.
def set_desired(self, desired):
self.set_point = desired
def update(self, measurement):
# proportional error
self.error = self.set_point - measurement
# integral error
self.integral += self.error
return self.Kp * self.error + self.Ki * self.integral
controller = SimplePIController(0.1, 0.002)
set_speed = 30
controller.set_desired(set_speed)
def crop_image(img, img_height=75, img_width=200):
height = img.shape[0]
width = img.shape[1]
y_start = 60
#x_start = int(width/2)-int(img_width/2)
return img[y_start:y_start+img_height, 0:width ]#x_start:x_start+img_width]
#sio.on('telemetry')
def telemetry(sid, data):
if data:
# The current steering angle of the car
steering_angle = data["steering_angle"]
# The current throttle of the car
throttle = data["throttle"]
# The current speed of the car
speed = data["speed"]
# The current image from the center camera of the car
imgString = data["image"]
image = Image.open(BytesIO(base64.b64decode(imgString)))
image_array = np.asarray(image)
image_array = crop_image(image_array)
steering_angle = float(model.predict(image_array[None, :, :, :], batch_size=1))
throttle = controller.update(float(speed))
print(steering_angle, throttle)
send_control(steering_angle, throttle)
# save frame
if args.image_folder != '':
timestamp = datetime.utcnow().strftime('%Y_%m_%d_%H_%M_%S_%f')[:-3]
image_filename = os.path.join(args.image_folder, timestamp)
image.save('{}.jpg'.format(image_filename))
else:
# NOTE: DON'T EDIT THIS.
sio.emit('manual', data={}, skip_sid=True)
#sio.on('connect')
def connect(sid, environ):
print("connect ", sid)
send_control(0, 0)
def send_control(steering_angle, throttle):
sio.emit(
"steer",
data={
'steering_angle': steering_angle.__str__(),
'throttle': throttle.__str__()
},
skip_sid=True)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Remote Driving')
parser.add_argument(
'model',
type=str,
help='Path to model h5 file. Model should be on the same path.'
)
parser.add_argument(
'image_folder',
type=str,
nargs='?',
default='',
help='Path to image folder. This is where the images from the run will be saved.'
)
args = parser.parse_args()
# check that model Keras version is same as local Keras version
f = h5py.File(args.model, mode='r')
model_version = f.attrs.get('keras_version')
keras_version = str(keras_version).encode('utf8')
if model_version != keras_version:
print('You are using Keras version ', keras_version,
', but the model was built using ', model_version)
model = load_model(args.model)
if args.image_folder != '':
print("Creating image folder at {}".format(args.image_folder))
if not os.path.exists(args.image_folder):
os.makedirs(args.image_folder)
else:
shutil.rmtree(args.image_folder)
os.makedirs(args.image_folder)
print("RECORDING THIS RUN ...")
else:
print("NOT RECORDING THIS RUN ...")
# wrap Flask application with engineio's middleware
app = socketio.Middleware(sio, app)
# deploy as an eventlet WSGI server
eventlet.wsgi.server(eventlet.listen(('', 4567)), app)
problem video link
https://youtu.be/nP8WH8pM29Q
This is due to the socketio version. Use 4.2.1, that should fix your problem
My node for ROS is supposed to subscribe to topic "'raspicam_node/image' (type sensor_msgs/Image)", and publish to both topics "'/image/segmented' (type sensor_msgs/Image)" and "'/image/centroid' (type geometry_msgs/PointStamped)".
Thank you for any and all help.
Other instructions for the question are
"This node should import the file image_processing.py to use all the
functions that you have developed so far. For each new image received from the camera,
the node should perform the same operations described in Question provided 2.3, but
without visualization of the results. Instead, the node should:
Publish the segmented image with the line on the topic /image/segmented .
Insert the computed horizontal centroid in the x field of a PointStamped message,
add the current time to the header.stamp field, and then publish the message
on the topic /image/centroid"
'''
#!/usr/bin/env python
"""This is a node to perform real-time extraction of the centroid of the line from images acquired by camera of robot and export data."""
import rospy
import numpy as np
import image_processing as ip
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from geometry_msgs.msg import PointStamped
import cv2
class ImageSegmentNode(object):
def __init__(self):
rospy.Subscriber("/raspicam_node/image", Image, callback=self.callback)
self.pub_r1 = rospy.Publisher("/image/segmented", Image, queue_size=10)
self.pub_r2 = rospy.Publisher("/image/centroid", PointStamped, queue_size=10)
self.bridge = CvBridge()
def callback(self, msg):
img = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
img = cv2.imread(img)
lb, ub = ip.classifier_parameters()
img_seg = cv2.inRange(img, lb, ub)
x = ip.image_centroid_horizontal(img_seg)
img_seg = ip.image_one_to_three_channels(img_seg)
img_seg = ip.image_line_vertical(img_seg, x)
self.pub_r1.publish(img_seg)
point1 = PointStamped()
point1.point = x
point1.header = rospy.Time.now()
self.pub_r2.publish(point1)
if __name__ == '__main__':
rospy.init_node('image_segment_node')
isn = ImageSegmentNode()
rospy.loginfo("Node has been started.")
rospy.spin()
I'm trying to subscribe to a ROS node published by a vrep vision sensor. Here is my code, which works fine when using my built-in webcam:
import rospy
from sensor_msgs.msg import Image
from std_msgs.msg import String
from cv_bridge import CvBridge, CvBridgeError
import cv2
import numpy as np
import tensorflow as tf
import classify_image
class RosTensorFlow():
def __init__(self):
classify_image.maybe_download_and_extract()
self._session = tf.Session()
classify_image.create_graph()
self._cv_bridge = CvBridge()
self._sub = rospy.Subscriber('image', Image, self.callback, queue_size=1)
self._pub = rospy.Publisher('result', String, queue_size=1)
self.score_threshold = rospy.get_param('~score_threshold', 0.1)
self.use_top_k = rospy.get_param('~use_top_k', 5)
def callback(self, image_msg):
try:
cv_image = self._cv_bridge.imgmsg_to_cv2(image_msg, desired_encoding="passthrough")
except CvBridgeError as e:
print(e)
# copy from
# classify_image.py
image_data = cv2.imencode('.jpg', cv_image)[1].tostring()
# Creates graph from saved GraphDef.
softmax_tensor = self._session.graph.get_tensor_by_name('softmax:0')
predictions = self._session.run(
softmax_tensor, {'DecodeJpeg/contents:0': image_data})
predictions = np.squeeze(predictions)
# Creates node ID --> English string lookup.
node_lookup = classify_image.NodeLookup()
top_k = predictions.argsort()[-self.use_top_k:][::-1]
for node_id in top_k:
human_string = node_lookup.id_to_string(node_id)
score = predictions[node_id]
if score > self.score_threshold:
rospy.loginfo('%s (score = %.5f)' % (human_string, score))
self._pub.publish(human_string)
def main(self):
rospy.spin()
if __name__ == '__main__':
classify_image.setup_args()
rospy.init_node('rostensorflow')
tensor = RosTensorFlow()
tensor.main()
and my non-threaded child-script in vrep, which i basically copied from the rosInterfaceTopicPublisherAndSubscriber.ttt tutorial:
function sysCall_init()
activeVisionSensor=sim.getObjectHandle('Vision_sensor')
pub=simROS.advertise('image', 'sensor_msgs/Image')
simROS.publisherTreatUInt8ArrayAsString(pub) -- treat uint8 arrays as strings (much faster, tables/arrays are kind of slow in Lua)
end
function sysCall_sensing()
-- Publish the image of the active vision sensor:
local data,w,h=sim.getVisionSensorCharImage(activeVisionSensor)
d={}
d['header']={seq=0,stamp=simROS.getTime(), frame_id="a"}
d['height']=h
d['width']=w
d['encoding']='rgb8'
d['is_bigendian']=1
d['step']=w*3
d['data']=data
simROS.publish(pub,d)
print("published")
end
function sysCall_cleanup()
simROS.shutdownPublisher(pub)
end
I'm running the script with:
python image_recognition.py
I'm getting no error message, but there is just no output. Can you please give me a hint what I need to change? I did a lot of research but without any success so far.
Got it working.
For people interested, I was running:
rostopic list -v
which told me that the node was actually published as:
/vrep_ros_interface/image
I just changed that in
rospy.Subscriber
and the script worked :)
I'm trying to do some segmentation on a pointcloud from a kinect one in ROS. As of now i have this:
import rospy
import pcl
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
def on_new_point_cloud(data):
pc = pc2.read_points(data, skip_nans=True, field_names=("x", "y", "z"))
pc_list = []
for p in pc:
pc_list.append( [p[0],p[1],p[2]] )
p = pcl.PointCloud()
p.from_list(pc_list)
seg = p.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
indices, model = seg.segment()
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/kinect2/hd/points", PointCloud2, on_new_point_cloud)
rospy.spin()
This seems to work but is very slow because of the for loop.
My questions are:
1) How do i effeciently convert from the PointCloud2 message to a pcl pointcloud
2) How do i visualize the clouds.
import rospy
import pcl
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import ros_numpy
def callback(data):
pc = ros_numpy.numpify(data)
points=np.zeros((pc.shape[0],3))
points[:,0]=pc['x']
points[:,1]=pc['y']
points[:,2]=pc['z']
p = pcl.PointCloud(np.array(points, dtype=np.float32))
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("/velodyne_points", PointCloud2, callback)
rospy.spin()
I would prefer using ros_numpy module to first convert to numpy array and initialize Point Cloud from that array.
On Ubuntu 20.04 with Python3 I use the following:
import numpy as np
import pcl # pip3 install python-pcl
import ros_numpy # apt install ros-noetic-ros-numpy
import rosbag
import sensor_msgs
def convert_pc_msg_to_np(pc_msg):
# Fix rosbag issues, see: https://github.com/eric-wieser/ros_numpy/issues/23
pc_msg.__class__ = sensor_msgs.msg._PointCloud2.PointCloud2
offset_sorted = {f.offset: f for f in pc_msg.fields}
pc_msg.fields = [f for (_, f) in sorted(offset_sorted.items())]
# Conversion from PointCloud2 msg to np array.
pc_np = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(pc_msg, remove_nans=True)
pc_pcl = pcl.PointCloud(np.array(pc_np, dtype=np.float32))
return pc_np, pc_pcl # point cloud in numpy and pcl format
# Use a ros subscriber as you already suggested or is shown in the other
# answers to run it online :)
# To run it offline on a rosbag use:
for topic, msg, t in rosbag.Bag('/my/rosbag.bag').read_messages():
if topic == "/my/cloud":
pc_np, pc_pcl = convert_pc_msg_to_np(msg)
This works for me. I just resize the point cloud since mine is an ordered pc (512x x 512px). My code is adapted from #Abdulbaki Aybakan - thanks for this!
My code:
pc = ros_numpy.numpify(pointcloud)
height = pc.shape[0]
width = pc.shape[1]
np_points = np.zeros((height * width, 3), dtype=np.float32)
np_points[:, 0] = np.resize(pc['x'], height * width)
np_points[:, 1] = np.resize(pc['y'], height * width)
np_points[:, 2] = np.resize(pc['z'], height * width)
To use ros_numpy one has to clone the repo: http://wiki.ros.org/ros_numpy