diff --git a/src/nautilus_launch/launch/full_system.launch b/src/nautilus_launch/launch/full_system.launch index fbc610d..5f7b14c 100644 --- a/src/nautilus_launch/launch/full_system.launch +++ b/src/nautilus_launch/launch/full_system.launch @@ -6,7 +6,7 @@ - + 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/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/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..8618fae 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,39 @@ 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): + image = image.data # A bad thing but quick fix + 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/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/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 + + + + + + + + diff --git a/src/uwrov_cameras/stream_topic_relay.py b/src/uwrov_cameras/stream_topic_relay.py new file mode 100755 index 0000000..5661c2d --- /dev/null +++ b/src/uwrov_cameras/stream_topic_relay.py @@ -0,0 +1,61 @@ +#!/usr/bin/env python3 +import rospy +import os +from sensor_msgs.msg import CompressedImage +import cv2 +import numpy as np + +def main(): + 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 = {'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 = {} + 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) + if (ips_to_topics[ip] == '/rov_camera/front'): + pairs[stream] = (publisher, False) + else: + pairs[stream] = (publisher, False) + + msg = CompressedImage() + + while not rospy.is_shutdown(): + for stream in pairs: + 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.tolist() + else: + #print(frame.flatten().tolist()) + msg.data = frame.flatten().tolist() + + if ret: + pairs[stream][0].publish(msg) + except Exception as e: + print(e) + pass + rate.sleep() + +def shutdown_fn(pairs_ptr): + pairs = pairs_ptr[0] + for stream in pairs: + stream.release() + print('shutting down') + +if __name__ == '__main__': + main() 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 +
+ ); + } +} 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('') + ); 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)