From 293afa6cfd5a16c3f7e3c8d0fabae3202bead6b4 Mon Sep 17 00:00:00 2001 From: wang4 Date: Sun, 27 Feb 2022 18:57:50 -0800 Subject: [PATCH 01/13] Added motion to rostopic script --- src/uwrov_cameras/CMakeLists.txt | 207 +++++++++++++++++++++++++++ src/uwrov_cameras/motion_to_topic.py | 31 ++++ src/uwrov_cameras/package.xml | 71 +++++++++ 3 files changed, 309 insertions(+) create mode 100644 src/uwrov_cameras/CMakeLists.txt create mode 100644 src/uwrov_cameras/motion_to_topic.py create mode 100644 src/uwrov_cameras/package.xml diff --git a/src/uwrov_cameras/CMakeLists.txt b/src/uwrov_cameras/CMakeLists.txt new file mode 100644 index 0000000..658d801 --- /dev/null +++ b/src/uwrov_cameras/CMakeLists.txt @@ -0,0 +1,207 @@ +cmake_minimum_required(VERSION 3.0.2) +project(uwrov_cameras) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + sensor_msgs + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# sensor_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES uwrov_cameras +# CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/uwrov_cameras.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/uwrov_cameras_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_uwrov_cameras.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/uwrov_cameras/motion_to_topic.py b/src/uwrov_cameras/motion_to_topic.py new file mode 100644 index 0000000..6180cf0 --- /dev/null +++ b/src/uwrov_cameras/motion_to_topic.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +import picamera +import rospy +import os +from sensor_msgs.msg import CompressedImage +import cv2 +from cv_bridge import CvBridge + +def main(): + src = 'ip' + stream = cv2.VideoCapture(src) + rospy.init_node('motion') + rospy.on_shutdown(shutdown_fn) + + publisher = rospy.Publisher('/nautilus/motion', Image, queue_size=1) + + rate = rospy.Rate(100) + br = CvBridge() + + while not rospy.is_shutdown(): + ret, frame = stream.read() + if ret: + publisher.publish(br.cv2_to_imgmsg(frame)) + rate.sleep() + +def shutdown_fn(): + stream.release() + print('shutting down') + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/uwrov_cameras/package.xml b/src/uwrov_cameras/package.xml new file mode 100644 index 0000000..c1f35a7 --- /dev/null +++ b/src/uwrov_cameras/package.xml @@ -0,0 +1,71 @@ + + + uwrov_cameras + 0.0.0 + The uwrov_cameras package + + + + + root + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + sensor_msgs + std_msgs + roscpp + rospy + sensor_msgs + std_msgs + roscpp + rospy + sensor_msgs + std_msgs + + + + + + + + From f96632ef0537b16e87a6d44f85643b8653efb104 Mon Sep 17 00:00:00 2001 From: wang4 Date: Sun, 27 Mar 2022 21:36:57 -0700 Subject: [PATCH 02/13] Updated stream topic relay for multiple cameras --- src/uwrov_cameras/motion_to_topic.py | 31 ---- src/uwrov_cameras/stream_topic_relay.py | 39 ++++ .../src/components/ipCamera/IpCamera.js | 172 +++++++++--------- 3 files changed, 125 insertions(+), 117 deletions(-) delete mode 100644 src/uwrov_cameras/motion_to_topic.py create mode 100644 src/uwrov_cameras/stream_topic_relay.py diff --git a/src/uwrov_cameras/motion_to_topic.py b/src/uwrov_cameras/motion_to_topic.py deleted file mode 100644 index 6180cf0..0000000 --- a/src/uwrov_cameras/motion_to_topic.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python3 -import picamera -import rospy -import os -from sensor_msgs.msg import CompressedImage -import cv2 -from cv_bridge import CvBridge - -def main(): - src = 'ip' - stream = cv2.VideoCapture(src) - rospy.init_node('motion') - rospy.on_shutdown(shutdown_fn) - - publisher = rospy.Publisher('/nautilus/motion', Image, queue_size=1) - - rate = rospy.Rate(100) - br = CvBridge() - - while not rospy.is_shutdown(): - ret, frame = stream.read() - if ret: - publisher.publish(br.cv2_to_imgmsg(frame)) - rate.sleep() - -def shutdown_fn(): - stream.release() - print('shutting down') - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py new file mode 100644 index 0000000..4c2eec1 --- /dev/null +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +import picamera +import rospy +import os +from sensor_msgs.msg import CompressedImage +import cv2 + +def main(): + ips_to_topics = {'192.168.0.99:8081' : '/nautilus/motion1', + '192.168.0.99:8082' : '/nautilus/motion2'} + + rospy.init_node('motion') + pairs = {} + rospy.on_shutdown(shutdown_fn, [pairs]) + rate = rospy.Rate(60) # framerate - consider lowering + + for ip in ips_to_topics: + stream = cv2.VideoCapture(ip) + publisher = rospy.Publisher(ips_to_topics[ip], CompressedImage, queue_size=1) + pairs[stream] = publisher + + msg = CompressedImage() + + while not rospy.is_shutdown(): + for stream in pairs: + ret, frame = stream.read() + msg.data = frame + if ret: + pairs[stream].publish(msg) + rate.sleep() + +def shutdown_fn(pairs_ptr): + pairs = pairs_ptr[0] + for stream in pairs: + stream.release() + print('shutting down') + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/uwrov_interface/src/components/ipCamera/IpCamera.js b/src/uwrov_interface/src/components/ipCamera/IpCamera.js index a48dea8..0eaa27d 100644 --- a/src/uwrov_interface/src/components/ipCamera/IpCamera.js +++ b/src/uwrov_interface/src/components/ipCamera/IpCamera.js @@ -1,86 +1,86 @@ -import React from "react"; -import "./IpCamera.css"; - -export default class Camera extends React.Component { - constructor(props) { - super(props); - this.socket = require("socket.io-client")("http://localhost:4040"); - this.state = { - img_src: - "https://cdn.mos.cms.futurecdn.net/42E9as7NaTaAi4A6JcuFwG-1200-80.jpg", - ip_add_pre: null, - ip_add_post: null, - hide: false, - }; - - console.log(this.socket); - this.socket.on("Image Display", (image) => { - console.log("got image"); - this.setImage(image); - }); - } - - setImage = (image) => { - let typed_array = new Uint8Array(image.image); - const data = typed_array.reduce( - (acc, i) => (acc += String.fromCharCode.apply(null, [i])), - "" - ); - //const string_char = String.fromCharCode.apply(null, typed_array); - let imageurl = "data:image/png;base64, " + data; - this.setState({ img_src: imageurl }); - }; - - render() { - return ( -
-
- -
- {!this.state.hide ? ( - { - this.setState({ - ip_add_pre: e.target.value, - }); - }} - /> - ) : null} - {!this.state.hide ? ( -
{ - this.setState({ - ip_add_post: this.state.ip_add_pre, - }); - }} - > - {" "} - Submit{" "} -
- ) : null} -
-
- IP Camera -
- ); - } -} +import React from "react"; +import "./IpCamera.css"; + +export default class Camera extends React.Component { + constructor(props) { + super(props); + this.socket = require("socket.io-client")("http://localhost:4040"); + this.state = { + img_src: + "http://192.168.0.99:8081", + ip_add_pre: null, + ip_add_post: null, + hide: false, + }; + + console.log(this.socket); + this.socket.on("Image Display", (image) => { + console.log("got image"); + this.setImage(image); + }); + } + + setImage = (image) => { + let typed_array = new Uint8Array(image.image); + const data = typed_array.reduce( + (acc, i) => (acc += String.fromCharCode.apply(null, [i])), + "" + ); + //const string_char = String.fromCharCode.apply(null, typed_array); + let imageurl = "data:image/png;base64, " + data; + this.setState({ img_src: imageurl }); + }; + + render() { + return ( +
+
+ +
+ {!this.state.hide ? ( + { + this.setState({ + ip_add_pre: e.target.value, + }); + }} + /> + ) : null} + {!this.state.hide ? ( +
{ + this.setState({ + ip_add_post: this.state.ip_add_pre, + }); + }} + > + {" "} + Submit{" "} +
+ ) : null} +
+
+ IP Camera +
+ ); + } +} From 53882ece14a3d7b70d2dc1554808baf532a92530 Mon Sep 17 00:00:00 2001 From: Angela Wei Date: Mon, 23 May 2022 20:02:18 -0700 Subject: [PATCH 03/13] Updated stream topic relay for fisheye cameras --- src/uwrov_cameras/stream_topic_relay.py | 27 ++++++++++++++++++++----- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py index 4c2eec1..6ccc14f 100644 --- a/src/uwrov_cameras/stream_topic_relay.py +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -4,10 +4,17 @@ import os from sensor_msgs.msg import CompressedImage import cv2 +import numpy as np def main(): - ips_to_topics = {'192.168.0.99:8081' : '/nautilus/motion1', - '192.168.0.99:8082' : '/nautilus/motion2'} + DIM=(640, 480) + K=np.array([[232.59834329560857, 0.0, 323.69056666007043], [0.0, 224.2212045670311, 249.45648108995732], [0.0, 0.0, 1.0]]) + D=np.array([[-0.030436347449448193], [0.009118483708036189], [-0.03442166148483811], [0.014312258830753885]]) + + ips_to_topics = {'192.168.0.99:8081' : '/rov_camera/front', + '192.168.0.99:8082' : '/rov_camera/down', + '192.168.0.99:8083' : '/rov_camera/left', + '192.168.0.99:8084' : '/rov_camera/right'} rospy.init_node('motion') pairs = {} @@ -17,16 +24,26 @@ def main(): for ip in ips_to_topics: stream = cv2.VideoCapture(ip) publisher = rospy.Publisher(ips_to_topics[ip], CompressedImage, queue_size=1) - pairs[stream] = publisher + if (ips_to_topics[ip] == '/rov_camera/front'): + pairs[stream] = (publisher, True) + else: + pairs[stream] = (publisher, False) msg = CompressedImage() while not rospy.is_shutdown(): for stream in pairs: ret, frame = stream.read() - msg.data = frame + if pairs[stream][1]: + h,w = frame.shape[:2] + map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) + undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) + msg.data = undistorted_img + else: + msg.data = frame + if ret: - pairs[stream].publish(msg) + pairs[stream][0].publish(msg) rate.sleep() def shutdown_fn(pairs_ptr): From 9172f07b4e20b8734f27dda20c413db6bc85b168 Mon Sep 17 00:00:00 2001 From: wang4 Date: Sun, 27 Feb 2022 18:57:50 -0800 Subject: [PATCH 04/13] Added motion to rostopic script --- src/uwrov_cameras/CMakeLists.txt | 207 +++++++++++++++++++++++++++ src/uwrov_cameras/motion_to_topic.py | 31 ++++ src/uwrov_cameras/package.xml | 71 +++++++++ 3 files changed, 309 insertions(+) create mode 100644 src/uwrov_cameras/CMakeLists.txt create mode 100644 src/uwrov_cameras/motion_to_topic.py create mode 100644 src/uwrov_cameras/package.xml diff --git a/src/uwrov_cameras/CMakeLists.txt b/src/uwrov_cameras/CMakeLists.txt new file mode 100644 index 0000000..658d801 --- /dev/null +++ b/src/uwrov_cameras/CMakeLists.txt @@ -0,0 +1,207 @@ +cmake_minimum_required(VERSION 3.0.2) +project(uwrov_cameras) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + sensor_msgs + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# sensor_msgs# std_msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES uwrov_cameras +# CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/uwrov_cameras.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/uwrov_cameras_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_uwrov_cameras.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/uwrov_cameras/motion_to_topic.py b/src/uwrov_cameras/motion_to_topic.py new file mode 100644 index 0000000..6180cf0 --- /dev/null +++ b/src/uwrov_cameras/motion_to_topic.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +import picamera +import rospy +import os +from sensor_msgs.msg import CompressedImage +import cv2 +from cv_bridge import CvBridge + +def main(): + src = 'ip' + stream = cv2.VideoCapture(src) + rospy.init_node('motion') + rospy.on_shutdown(shutdown_fn) + + publisher = rospy.Publisher('/nautilus/motion', Image, queue_size=1) + + rate = rospy.Rate(100) + br = CvBridge() + + while not rospy.is_shutdown(): + ret, frame = stream.read() + if ret: + publisher.publish(br.cv2_to_imgmsg(frame)) + rate.sleep() + +def shutdown_fn(): + stream.release() + print('shutting down') + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/uwrov_cameras/package.xml b/src/uwrov_cameras/package.xml new file mode 100644 index 0000000..c1f35a7 --- /dev/null +++ b/src/uwrov_cameras/package.xml @@ -0,0 +1,71 @@ + + + uwrov_cameras + 0.0.0 + The uwrov_cameras package + + + + + root + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + sensor_msgs + std_msgs + roscpp + rospy + sensor_msgs + std_msgs + roscpp + rospy + sensor_msgs + std_msgs + + + + + + + + From d244ea8de6590590436bca2b9f4f4ee79c8d598b Mon Sep 17 00:00:00 2001 From: wang4 Date: Sun, 27 Mar 2022 21:36:57 -0700 Subject: [PATCH 05/13] Updated stream topic relay for multiple cameras --- src/uwrov_cameras/motion_to_topic.py | 31 ---- src/uwrov_cameras/stream_topic_relay.py | 39 ++++ .../src/components/ipCamera/IpCamera.js | 172 +++++++++--------- 3 files changed, 125 insertions(+), 117 deletions(-) delete mode 100644 src/uwrov_cameras/motion_to_topic.py create mode 100644 src/uwrov_cameras/stream_topic_relay.py diff --git a/src/uwrov_cameras/motion_to_topic.py b/src/uwrov_cameras/motion_to_topic.py deleted file mode 100644 index 6180cf0..0000000 --- a/src/uwrov_cameras/motion_to_topic.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python3 -import picamera -import rospy -import os -from sensor_msgs.msg import CompressedImage -import cv2 -from cv_bridge import CvBridge - -def main(): - src = 'ip' - stream = cv2.VideoCapture(src) - rospy.init_node('motion') - rospy.on_shutdown(shutdown_fn) - - publisher = rospy.Publisher('/nautilus/motion', Image, queue_size=1) - - rate = rospy.Rate(100) - br = CvBridge() - - while not rospy.is_shutdown(): - ret, frame = stream.read() - if ret: - publisher.publish(br.cv2_to_imgmsg(frame)) - rate.sleep() - -def shutdown_fn(): - stream.release() - print('shutting down') - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py new file mode 100644 index 0000000..4c2eec1 --- /dev/null +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -0,0 +1,39 @@ +#!/usr/bin/env python3 +import picamera +import rospy +import os +from sensor_msgs.msg import CompressedImage +import cv2 + +def main(): + ips_to_topics = {'192.168.0.99:8081' : '/nautilus/motion1', + '192.168.0.99:8082' : '/nautilus/motion2'} + + rospy.init_node('motion') + pairs = {} + rospy.on_shutdown(shutdown_fn, [pairs]) + rate = rospy.Rate(60) # framerate - consider lowering + + for ip in ips_to_topics: + stream = cv2.VideoCapture(ip) + publisher = rospy.Publisher(ips_to_topics[ip], CompressedImage, queue_size=1) + pairs[stream] = publisher + + msg = CompressedImage() + + while not rospy.is_shutdown(): + for stream in pairs: + ret, frame = stream.read() + msg.data = frame + if ret: + pairs[stream].publish(msg) + rate.sleep() + +def shutdown_fn(pairs_ptr): + pairs = pairs_ptr[0] + for stream in pairs: + stream.release() + print('shutting down') + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/uwrov_interface/src/components/ipCamera/IpCamera.js b/src/uwrov_interface/src/components/ipCamera/IpCamera.js index a48dea8..0eaa27d 100644 --- a/src/uwrov_interface/src/components/ipCamera/IpCamera.js +++ b/src/uwrov_interface/src/components/ipCamera/IpCamera.js @@ -1,86 +1,86 @@ -import React from "react"; -import "./IpCamera.css"; - -export default class Camera extends React.Component { - constructor(props) { - super(props); - this.socket = require("socket.io-client")("http://localhost:4040"); - this.state = { - img_src: - "https://cdn.mos.cms.futurecdn.net/42E9as7NaTaAi4A6JcuFwG-1200-80.jpg", - ip_add_pre: null, - ip_add_post: null, - hide: false, - }; - - console.log(this.socket); - this.socket.on("Image Display", (image) => { - console.log("got image"); - this.setImage(image); - }); - } - - setImage = (image) => { - let typed_array = new Uint8Array(image.image); - const data = typed_array.reduce( - (acc, i) => (acc += String.fromCharCode.apply(null, [i])), - "" - ); - //const string_char = String.fromCharCode.apply(null, typed_array); - let imageurl = "data:image/png;base64, " + data; - this.setState({ img_src: imageurl }); - }; - - render() { - return ( -
-
- -
- {!this.state.hide ? ( - { - this.setState({ - ip_add_pre: e.target.value, - }); - }} - /> - ) : null} - {!this.state.hide ? ( -
{ - this.setState({ - ip_add_post: this.state.ip_add_pre, - }); - }} - > - {" "} - Submit{" "} -
- ) : null} -
-
- IP Camera -
- ); - } -} +import React from "react"; +import "./IpCamera.css"; + +export default class Camera extends React.Component { + constructor(props) { + super(props); + this.socket = require("socket.io-client")("http://localhost:4040"); + this.state = { + img_src: + "http://192.168.0.99:8081", + ip_add_pre: null, + ip_add_post: null, + hide: false, + }; + + console.log(this.socket); + this.socket.on("Image Display", (image) => { + console.log("got image"); + this.setImage(image); + }); + } + + setImage = (image) => { + let typed_array = new Uint8Array(image.image); + const data = typed_array.reduce( + (acc, i) => (acc += String.fromCharCode.apply(null, [i])), + "" + ); + //const string_char = String.fromCharCode.apply(null, typed_array); + let imageurl = "data:image/png;base64, " + data; + this.setState({ img_src: imageurl }); + }; + + render() { + return ( +
+
+ +
+ {!this.state.hide ? ( + { + this.setState({ + ip_add_pre: e.target.value, + }); + }} + /> + ) : null} + {!this.state.hide ? ( +
{ + this.setState({ + ip_add_post: this.state.ip_add_pre, + }); + }} + > + {" "} + Submit{" "} +
+ ) : null} +
+
+ IP Camera +
+ ); + } +} From 2b4ebf083845b414582937e79c74af1cfefc6e2d Mon Sep 17 00:00:00 2001 From: Angela Wei Date: Mon, 23 May 2022 20:02:18 -0700 Subject: [PATCH 06/13] Updated stream topic relay for fisheye cameras --- src/uwrov_cameras/stream_topic_relay.py | 27 ++++++++++++++++++++----- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py index 4c2eec1..6ccc14f 100644 --- a/src/uwrov_cameras/stream_topic_relay.py +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -4,10 +4,17 @@ import os from sensor_msgs.msg import CompressedImage import cv2 +import numpy as np def main(): - ips_to_topics = {'192.168.0.99:8081' : '/nautilus/motion1', - '192.168.0.99:8082' : '/nautilus/motion2'} + DIM=(640, 480) + K=np.array([[232.59834329560857, 0.0, 323.69056666007043], [0.0, 224.2212045670311, 249.45648108995732], [0.0, 0.0, 1.0]]) + D=np.array([[-0.030436347449448193], [0.009118483708036189], [-0.03442166148483811], [0.014312258830753885]]) + + ips_to_topics = {'192.168.0.99:8081' : '/rov_camera/front', + '192.168.0.99:8082' : '/rov_camera/down', + '192.168.0.99:8083' : '/rov_camera/left', + '192.168.0.99:8084' : '/rov_camera/right'} rospy.init_node('motion') pairs = {} @@ -17,16 +24,26 @@ def main(): for ip in ips_to_topics: stream = cv2.VideoCapture(ip) publisher = rospy.Publisher(ips_to_topics[ip], CompressedImage, queue_size=1) - pairs[stream] = publisher + if (ips_to_topics[ip] == '/rov_camera/front'): + pairs[stream] = (publisher, True) + else: + pairs[stream] = (publisher, False) msg = CompressedImage() while not rospy.is_shutdown(): for stream in pairs: ret, frame = stream.read() - msg.data = frame + if pairs[stream][1]: + h,w = frame.shape[:2] + map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) + undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) + msg.data = undistorted_img + else: + msg.data = frame + if ret: - pairs[stream].publish(msg) + pairs[stream][0].publish(msg) rate.sleep() def shutdown_fn(pairs_ptr): From c7f9d8370409201469668d871c4d7941a8705cf5 Mon Sep 17 00:00:00 2001 From: Andrew Date: Fri, 17 Jun 2022 16:21:13 -0700 Subject: [PATCH 07/13] Adding image relay functionality to interface --- src/nautilus_launch/launch/full_system.launch | 3 +- src/nautilus_launch/launch/surface_dev.launch | 1 + src/nautilus_motors/scripts/pwm_publisher.py | 0 src/robot_module/src/robot_module/main.py | 25 ++++++++---- .../src/robot_module/services/images.py | 39 +++++++++++++++++++ src/uwrov_cameras/stream_topic_relay.py | 32 ++++++++------- src/uwrov_server/scripts/main_server.py | 10 ++++- 7 files changed, 85 insertions(+), 25 deletions(-) mode change 100644 => 100755 src/nautilus_motors/scripts/pwm_publisher.py mode change 100644 => 100755 src/uwrov_cameras/stream_topic_relay.py diff --git a/src/nautilus_launch/launch/full_system.launch b/src/nautilus_launch/launch/full_system.launch index fbc610d..4e586c8 100644 --- a/src/nautilus_launch/launch/full_system.launch +++ b/src/nautilus_launch/launch/full_system.launch @@ -6,8 +6,9 @@ - + + diff --git a/src/nautilus_launch/launch/surface_dev.launch b/src/nautilus_launch/launch/surface_dev.launch index 11a0acd..eeb0e24 100644 --- a/src/nautilus_launch/launch/surface_dev.launch +++ b/src/nautilus_launch/launch/surface_dev.launch @@ -4,5 +4,6 @@ + diff --git a/src/nautilus_motors/scripts/pwm_publisher.py b/src/nautilus_motors/scripts/pwm_publisher.py old mode 100644 new mode 100755 diff --git a/src/robot_module/src/robot_module/main.py b/src/robot_module/src/robot_module/main.py index 724b8de..cf1b3be 100644 --- a/src/robot_module/src/robot_module/main.py +++ b/src/robot_module/src/robot_module/main.py @@ -42,7 +42,7 @@ def __run_if_service(self, service, func): """ try: if service in self.active_services: - func() + return func() else: print(f"[ERROR]: {service} not active yet") except Exception as e: @@ -148,18 +148,29 @@ def set_accel(self, vector): # TODO: Implement pass + def get_image_topics(self): + return self.__run_if_service("images", + lambda: self.active_services["images"] + .get_image_topics() + ) - def get_image(self, index=0): + def get_image(self, topic): """ Returns the current camera image available from the robot. :param int: index of the image """ - # TODO: Implement - pass + return self.__run_if_service("images", + lambda: self.active_services["images"] + .get_image(topic) + ) - def add_image_listener(self, listener, index=0): + def add_image_listener(self, listener): """ - Ret + add a callback function for the image stream. Listener is called on update + of the image. """ - pass + self.__run_if_service("images", + lambda: self.active_services["images"] + .callbacks.append(listener) + ) diff --git a/src/robot_module/src/robot_module/services/images.py b/src/robot_module/src/robot_module/services/images.py index a27a51d..5d9fb3a 100644 --- a/src/robot_module/src/robot_module/services/images.py +++ b/src/robot_module/src/robot_module/services/images.py @@ -3,9 +3,16 @@ #!/usr/bin/env python3 import rospy +from sensor_msgs.msg import CompressedImage from .service import Service +image_topics = [ + '/rov_camera/front', + '/rov_camera/down', + '/rov_camera/left', + '/rov_camera/right' + ] class RobotImages(Service): @@ -15,6 +22,38 @@ class RobotImages(Service): def __init__(self, node_name): super().__init__(node_name) + self.images = dict.fromkeys(image_topics, None) + self.callbacks = [] + self.subs = {} + + for t in image_topics: + self.subs[t] = rospy.Subscriber(t, CompressedImage, + self._handle_callbacks, + (t), + queue_size=1) def setup(self): pass + + def _handle_callbacks(self, image, topic): + self._update_image(image, topic) + for func in self.callbacks: + try: + func(image, topic) + except Exception as e: + print(e) + + def _update_image(self, image, topic): + self.images[topic] = image + + + def get_image(self, topic): + try: + return self.images[topic] + except Exception as e: + print(e) + return None + + + def get_image_topics(self): + return image_topics diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py old mode 100644 new mode 100755 index 6ccc14f..aef63f5 --- a/src/uwrov_cameras/stream_topic_relay.py +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import picamera import rospy import os from sensor_msgs.msg import CompressedImage @@ -18,9 +17,9 @@ def main(): rospy.init_node('motion') pairs = {} - rospy.on_shutdown(shutdown_fn, [pairs]) + rospy.on_shutdown(lambda: shutdown_fn([pairs])) rate = rospy.Rate(60) # framerate - consider lowering - + for ip in ips_to_topics: stream = cv2.VideoCapture(ip) publisher = rospy.Publisher(ips_to_topics[ip], CompressedImage, queue_size=1) @@ -33,17 +32,20 @@ def main(): while not rospy.is_shutdown(): for stream in pairs: - ret, frame = stream.read() - if pairs[stream][1]: - h,w = frame.shape[:2] - map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) - undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) - msg.data = undistorted_img - else: - msg.data = frame - - if ret: - pairs[stream][0].publish(msg) + try: + ret, frame = stream.read() + if pairs[stream][1]: + h, w = frame.shape[:2] + map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) + undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) + msg.data = undistorted_img + else: + msg.data = frame + + if ret: + pairs[stream][0].publish(msg) + except Exception as e: + pass rate.sleep() def shutdown_fn(pairs_ptr): @@ -53,4 +55,4 @@ def shutdown_fn(pairs_ptr): print('shutting down') if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/src/uwrov_server/scripts/main_server.py b/src/uwrov_server/scripts/main_server.py index 93965e4..7542995 100755 --- a/src/uwrov_server/scripts/main_server.py +++ b/src/uwrov_server/scripts/main_server.py @@ -31,8 +31,11 @@ @sio.on("Get IDs") def send_image_id(): - #sio.emit("IDs", {'ids': image_handles}, broadcast=True) - pass + sio.emit("IDs", {'ids': robot.get_image_topics()}, broadcast=True) + + +def send_image(image, topic): + sio.emit("Image Display", {"id": topic, "image": image}, broadcast=True) @sio.on("Send Movement") @@ -61,9 +64,12 @@ def shutdown_server(signum, frame): if __name__ == '__main__': """ Sets up rospy and starts servers """ robot = RobotModule("surface") + robot.setup("images") robot.setup("movement") robot.setup("manipulator") + robot.add_image_listener(send_image) + # Define a way to exit gracefully signal.signal(signal.SIGINT, shutdown_server) From 343ace8678c78392de584f9ba0e7e9c89928fc54 Mon Sep 17 00:00:00 2001 From: uwrov-github Date: Fri, 17 Jun 2022 17:15:37 -0700 Subject: [PATCH 08/13] changing some stuff --- src/nautilus_launch/launch/full_system.launch | 1 - src/uwrov_cameras/stream_topic_relay.py | 14 ++++++++------ 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/nautilus_launch/launch/full_system.launch b/src/nautilus_launch/launch/full_system.launch index 4e586c8..5f7b14c 100644 --- a/src/nautilus_launch/launch/full_system.launch +++ b/src/nautilus_launch/launch/full_system.launch @@ -8,7 +8,6 @@ - diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py index aef63f5..f7037e5 100755 --- a/src/uwrov_cameras/stream_topic_relay.py +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -10,10 +10,11 @@ def main(): K=np.array([[232.59834329560857, 0.0, 323.69056666007043], [0.0, 224.2212045670311, 249.45648108995732], [0.0, 0.0, 1.0]]) D=np.array([[-0.030436347449448193], [0.009118483708036189], [-0.03442166148483811], [0.014312258830753885]]) - ips_to_topics = {'192.168.0.99:8081' : '/rov_camera/front', - '192.168.0.99:8082' : '/rov_camera/down', - '192.168.0.99:8083' : '/rov_camera/left', - '192.168.0.99:8084' : '/rov_camera/right'} + ips_to_topics = {'http://192.168.0.99:8081' : '/rov_camera/front', + 'http://192.168.0.99:8082' : '/rov_camera/down', + #'http://192.168.0.99:8083' : '/rov_camera/left', + #'http://192.168.0.99:8084' : '/rov_camera/right' + } rospy.init_node('motion') pairs = {} @@ -24,7 +25,7 @@ def main(): stream = cv2.VideoCapture(ip) publisher = rospy.Publisher(ips_to_topics[ip], CompressedImage, queue_size=1) if (ips_to_topics[ip] == '/rov_camera/front'): - pairs[stream] = (publisher, True) + pairs[stream] = (publisher, False) else: pairs[stream] = (publisher, False) @@ -35,7 +36,7 @@ def main(): try: ret, frame = stream.read() if pairs[stream][1]: - h, w = frame.shape[:2] + (h, w) = frame.shape[:2] map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) msg.data = undistorted_img @@ -45,6 +46,7 @@ def main(): if ret: pairs[stream][0].publish(msg) except Exception as e: + print(e) pass rate.sleep() From dc303ca59436a17da67cd92cca5da4d5bfd04860 Mon Sep 17 00:00:00 2001 From: wang4 Date: Sun, 27 Feb 2022 18:57:50 -0800 Subject: [PATCH 09/13] Added motion to rostopic script --- src/uwrov_cameras/motion_to_topic.py | 31 ++++++++++++++++++++++++++++ 1 file changed, 31 insertions(+) create mode 100644 src/uwrov_cameras/motion_to_topic.py diff --git a/src/uwrov_cameras/motion_to_topic.py b/src/uwrov_cameras/motion_to_topic.py new file mode 100644 index 0000000..6180cf0 --- /dev/null +++ b/src/uwrov_cameras/motion_to_topic.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +import picamera +import rospy +import os +from sensor_msgs.msg import CompressedImage +import cv2 +from cv_bridge import CvBridge + +def main(): + src = 'ip' + stream = cv2.VideoCapture(src) + rospy.init_node('motion') + rospy.on_shutdown(shutdown_fn) + + publisher = rospy.Publisher('/nautilus/motion', Image, queue_size=1) + + rate = rospy.Rate(100) + br = CvBridge() + + while not rospy.is_shutdown(): + ret, frame = stream.read() + if ret: + publisher.publish(br.cv2_to_imgmsg(frame)) + rate.sleep() + +def shutdown_fn(): + stream.release() + print('shutting down') + +if __name__ == '__main__': + main() \ No newline at end of file From 3532e7d079474b78d79428aa141b67e47b606b6d Mon Sep 17 00:00:00 2001 From: Andrew Date: Fri, 17 Jun 2022 17:21:54 -0700 Subject: [PATCH 10/13] Resolving Merge Conflicts --- src/uwrov_cameras/motion_to_topic.py | 31 ------------------------- src/uwrov_cameras/stream_topic_relay.py | 7 +++--- 2 files changed, 3 insertions(+), 35 deletions(-) delete mode 100644 src/uwrov_cameras/motion_to_topic.py diff --git a/src/uwrov_cameras/motion_to_topic.py b/src/uwrov_cameras/motion_to_topic.py deleted file mode 100644 index 6180cf0..0000000 --- a/src/uwrov_cameras/motion_to_topic.py +++ /dev/null @@ -1,31 +0,0 @@ -#!/usr/bin/env python3 -import picamera -import rospy -import os -from sensor_msgs.msg import CompressedImage -import cv2 -from cv_bridge import CvBridge - -def main(): - src = 'ip' - stream = cv2.VideoCapture(src) - rospy.init_node('motion') - rospy.on_shutdown(shutdown_fn) - - publisher = rospy.Publisher('/nautilus/motion', Image, queue_size=1) - - rate = rospy.Rate(100) - br = CvBridge() - - while not rospy.is_shutdown(): - ret, frame = stream.read() - if ret: - publisher.publish(br.cv2_to_imgmsg(frame)) - rate.sleep() - -def shutdown_fn(): - stream.release() - print('shutting down') - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py index 6ccc14f..17377c7 100644 --- a/src/uwrov_cameras/stream_topic_relay.py +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -1,5 +1,4 @@ #!/usr/bin/env python3 -import picamera import rospy import os from sensor_msgs.msg import CompressedImage @@ -20,7 +19,7 @@ def main(): pairs = {} rospy.on_shutdown(shutdown_fn, [pairs]) rate = rospy.Rate(60) # framerate - consider lowering - + for ip in ips_to_topics: stream = cv2.VideoCapture(ip) publisher = rospy.Publisher(ips_to_topics[ip], CompressedImage, queue_size=1) @@ -34,7 +33,7 @@ def main(): while not rospy.is_shutdown(): for stream in pairs: ret, frame = stream.read() - if pairs[stream][1]: + if pairs[stream][1]: h,w = frame.shape[:2] map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) @@ -53,4 +52,4 @@ def shutdown_fn(pairs_ptr): print('shutting down') if __name__ == '__main__': - main() \ No newline at end of file + main() From 04673f74b1765cba2d7ebd23bf2f48333da4bc26 Mon Sep 17 00:00:00 2001 From: Andrew Date: Fri, 17 Jun 2022 17:24:06 -0700 Subject: [PATCH 11/13] Resolving merge conflicts --- src/nautilus_launch/launch/full_system.launch | 3 +- src/nautilus_launch/launch/surface_dev.launch | 1 + src/nautilus_motors/scripts/pwm_publisher.py | 0 src/robot_module/src/robot_module/main.py | 25 ++++++++---- .../src/robot_module/services/images.py | 39 +++++++++++++++++++ src/uwrov_cameras/stream_topic_relay.py | 27 +++++++------ src/uwrov_server/scripts/main_server.py | 10 ++++- 7 files changed, 83 insertions(+), 22 deletions(-) mode change 100644 => 100755 src/nautilus_motors/scripts/pwm_publisher.py mode change 100644 => 100755 src/uwrov_cameras/stream_topic_relay.py diff --git a/src/nautilus_launch/launch/full_system.launch b/src/nautilus_launch/launch/full_system.launch index fbc610d..4e586c8 100644 --- a/src/nautilus_launch/launch/full_system.launch +++ b/src/nautilus_launch/launch/full_system.launch @@ -6,8 +6,9 @@ - + + diff --git a/src/nautilus_launch/launch/surface_dev.launch b/src/nautilus_launch/launch/surface_dev.launch index 11a0acd..eeb0e24 100644 --- a/src/nautilus_launch/launch/surface_dev.launch +++ b/src/nautilus_launch/launch/surface_dev.launch @@ -4,5 +4,6 @@ + diff --git a/src/nautilus_motors/scripts/pwm_publisher.py b/src/nautilus_motors/scripts/pwm_publisher.py old mode 100644 new mode 100755 diff --git a/src/robot_module/src/robot_module/main.py b/src/robot_module/src/robot_module/main.py index 724b8de..cf1b3be 100644 --- a/src/robot_module/src/robot_module/main.py +++ b/src/robot_module/src/robot_module/main.py @@ -42,7 +42,7 @@ def __run_if_service(self, service, func): """ try: if service in self.active_services: - func() + return func() else: print(f"[ERROR]: {service} not active yet") except Exception as e: @@ -148,18 +148,29 @@ def set_accel(self, vector): # TODO: Implement pass + def get_image_topics(self): + return self.__run_if_service("images", + lambda: self.active_services["images"] + .get_image_topics() + ) - def get_image(self, index=0): + def get_image(self, topic): """ Returns the current camera image available from the robot. :param int: index of the image """ - # TODO: Implement - pass + return self.__run_if_service("images", + lambda: self.active_services["images"] + .get_image(topic) + ) - def add_image_listener(self, listener, index=0): + def add_image_listener(self, listener): """ - Ret + add a callback function for the image stream. Listener is called on update + of the image. """ - pass + self.__run_if_service("images", + lambda: self.active_services["images"] + .callbacks.append(listener) + ) diff --git a/src/robot_module/src/robot_module/services/images.py b/src/robot_module/src/robot_module/services/images.py index a27a51d..5d9fb3a 100644 --- a/src/robot_module/src/robot_module/services/images.py +++ b/src/robot_module/src/robot_module/services/images.py @@ -3,9 +3,16 @@ #!/usr/bin/env python3 import rospy +from sensor_msgs.msg import CompressedImage from .service import Service +image_topics = [ + '/rov_camera/front', + '/rov_camera/down', + '/rov_camera/left', + '/rov_camera/right' + ] class RobotImages(Service): @@ -15,6 +22,38 @@ class RobotImages(Service): def __init__(self, node_name): super().__init__(node_name) + self.images = dict.fromkeys(image_topics, None) + self.callbacks = [] + self.subs = {} + + for t in image_topics: + self.subs[t] = rospy.Subscriber(t, CompressedImage, + self._handle_callbacks, + (t), + queue_size=1) def setup(self): pass + + def _handle_callbacks(self, image, topic): + self._update_image(image, topic) + for func in self.callbacks: + try: + func(image, topic) + except Exception as e: + print(e) + + def _update_image(self, image, topic): + self.images[topic] = image + + + def get_image(self, topic): + try: + return self.images[topic] + except Exception as e: + print(e) + return None + + + def get_image_topics(self): + return image_topics diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py old mode 100644 new mode 100755 index 17377c7..aef63f5 --- a/src/uwrov_cameras/stream_topic_relay.py +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -17,7 +17,7 @@ def main(): rospy.init_node('motion') pairs = {} - rospy.on_shutdown(shutdown_fn, [pairs]) + rospy.on_shutdown(lambda: shutdown_fn([pairs])) rate = rospy.Rate(60) # framerate - consider lowering for ip in ips_to_topics: @@ -32,17 +32,20 @@ def main(): while not rospy.is_shutdown(): for stream in pairs: - ret, frame = stream.read() - if pairs[stream][1]: - h,w = frame.shape[:2] - map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) - undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) - msg.data = undistorted_img - else: - msg.data = frame - - if ret: - pairs[stream][0].publish(msg) + try: + ret, frame = stream.read() + if pairs[stream][1]: + h, w = frame.shape[:2] + map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) + undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) + msg.data = undistorted_img + else: + msg.data = frame + + if ret: + pairs[stream][0].publish(msg) + except Exception as e: + pass rate.sleep() def shutdown_fn(pairs_ptr): diff --git a/src/uwrov_server/scripts/main_server.py b/src/uwrov_server/scripts/main_server.py index 93965e4..7542995 100755 --- a/src/uwrov_server/scripts/main_server.py +++ b/src/uwrov_server/scripts/main_server.py @@ -31,8 +31,11 @@ @sio.on("Get IDs") def send_image_id(): - #sio.emit("IDs", {'ids': image_handles}, broadcast=True) - pass + sio.emit("IDs", {'ids': robot.get_image_topics()}, broadcast=True) + + +def send_image(image, topic): + sio.emit("Image Display", {"id": topic, "image": image}, broadcast=True) @sio.on("Send Movement") @@ -61,9 +64,12 @@ def shutdown_server(signum, frame): if __name__ == '__main__': """ Sets up rospy and starts servers """ robot = RobotModule("surface") + robot.setup("images") robot.setup("movement") robot.setup("manipulator") + robot.add_image_listener(send_image) + # Define a way to exit gracefully signal.signal(signal.SIGINT, shutdown_server) From 95168082277e8440c9d68188b9dc371609e63f7e Mon Sep 17 00:00:00 2001 From: uwrov-github Date: Fri, 17 Jun 2022 18:27:14 -0700 Subject: [PATCH 12/13] changes --- src/nautilus_launch/launch/full_system.launch | 1 - .../scripts/go_forward/go_forward.py | 14 +++++++------- src/uwrov_cameras/stream_topic_relay.py | 5 +++-- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/nautilus_launch/launch/full_system.launch b/src/nautilus_launch/launch/full_system.launch index 4e586c8..5f7b14c 100644 --- a/src/nautilus_launch/launch/full_system.launch +++ b/src/nautilus_launch/launch/full_system.launch @@ -8,7 +8,6 @@ - diff --git a/src/nautilus_scripts/scripts/go_forward/go_forward.py b/src/nautilus_scripts/scripts/go_forward/go_forward.py index 5384817..0619222 100644 --- a/src/nautilus_scripts/scripts/go_forward/go_forward.py +++ b/src/nautilus_scripts/scripts/go_forward/go_forward.py @@ -7,16 +7,16 @@ def go_forward(): for i in range(10): - robot.set_vel([0, -0.1, 0], [0, 0, 0]) + robot.set_vel([0, -0.5, 0], [0, 0, 0]) time.sleep(2) robot.set_vel([0, 0, 0], [0, 0, 0]) time.sleep(1) - if i % 2 == 0: - robot.set_vel([0, 0, 0], [0, 0, -0.01]) - else: - robot.set_vel([0, 0, 0], [0, 0, 0.01]) - time.sleep(1) - robot.set_vel([0, 0, 0], [0, 0, 0]) + #if i % 2 == 0: + # robot.set_vel([0, 0, 0], [0, 0, -0.01]) + #else: + # robot.set_vel([0, 0, 0], [0, 0, 0.01]) + #time.sleep(1) + #robot.set_vel([0, 0, 0], [0, 0, 0]) def main(): diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py index 80cf890..703c9e3 100755 --- a/src/uwrov_cameras/stream_topic_relay.py +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -35,13 +35,14 @@ def main(): for stream in pairs: try: ret, frame = stream.read() + print(type(frame)) if pairs[stream][1]: h, w = frame.shape[:2] map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) - msg.data = undistorted_img + msg.data = undistorted_img.tolist() else: - msg.data = frame + msg.data = frame.tolist() if ret: pairs[stream][0].publish(msg) From 50e5f742cba87e2a9543dd2aca06f271b4fd5321 Mon Sep 17 00:00:00 2001 From: uwrov-github Date: Fri, 24 Jun 2022 00:42:40 -0700 Subject: [PATCH 13/13] Saving progress on changing image stream --- .../src/robot_module/services/images.py | 1 + src/uwrov_cameras/stream_topic_relay.py | 4 ++-- .../src/components/rosCamera/RosCamera.js | 21 ++++++++++++++++--- 3 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/robot_module/src/robot_module/services/images.py b/src/robot_module/src/robot_module/services/images.py index 5d9fb3a..8618fae 100644 --- a/src/robot_module/src/robot_module/services/images.py +++ b/src/robot_module/src/robot_module/services/images.py @@ -36,6 +36,7 @@ def setup(self): pass def _handle_callbacks(self, image, topic): + image = image.data # A bad thing but quick fix self._update_image(image, topic) for func in self.callbacks: try: diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py index 703c9e3..5661c2d 100755 --- a/src/uwrov_cameras/stream_topic_relay.py +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -35,14 +35,14 @@ def main(): for stream in pairs: try: ret, frame = stream.read() - print(type(frame)) if pairs[stream][1]: h, w = frame.shape[:2] map1, map2 = cv2.fisheye.initUndistortRectifyMap(K, D, np.eye(3), K, DIM, cv2.CV_16SC2) undistorted_img = cv2.remap(frame, map1, map2, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT) msg.data = undistorted_img.tolist() else: - msg.data = frame.tolist() + #print(frame.flatten().tolist()) + msg.data = frame.flatten().tolist() if ret: pairs[stream][0].publish(msg) diff --git a/src/uwrov_interface/src/components/rosCamera/RosCamera.js b/src/uwrov_interface/src/components/rosCamera/RosCamera.js index 4fce282..d61a9a2 100644 --- a/src/uwrov_interface/src/components/rosCamera/RosCamera.js +++ b/src/uwrov_interface/src/components/rosCamera/RosCamera.js @@ -127,9 +127,24 @@ export default class Camera extends React.Component { let decodeImageToURL = (image) => { let typed_array = new Uint8Array(image); + //console.log(typed_array) //const data = typed_array.reduce((acc, i) => acc += String.fromCharCode.apply(null, [i]), ''); - const data = btoa(String.fromCharCode.apply(null, typed_array)); + //const data = btoa(adata);//String.fromCharCode.apply(null, adata)); + const data = uint8ToBase64(typed_array); let imageurl = "data:image/png;base64, " + data; - console.log(imageurl); - return imageurl; + console.log(URL.createObjectURL( + new Blob([typed_array.buffer], { type: 'image/png' } /* (1) */) + )); + return URL.createObjectURL( + new Blob([typed_array.buffer], { type: 'image/png' } /* (1) */) + ); }; + +// StackOverflow +const uint8ToBase64 = (arr) => + btoa( + Array(arr.length) + .fill('') + .map((_, i) => String.fromCharCode(arr[i])) + .join('') + );