Skip to content

Commit

Permalink
Add camera publisher code (#83)
Browse files Browse the repository at this point in the history
* Set up 2 basic nodes

* Added cv_bridge

* Publishing works I think

* Added video test

* launch_only_docker connects to gui. Video output confirmed good

* Changed to compressed image

* Changed to compressed image

* stopped running camera test and slower streaming for testing

* Spin up a camera node for every camera detected

* Pushing any changes that were done. I am switching branches -vlad

* multi threaded executor for all cameras

* changed to individual topic for each camera

* bug fix to append string to string

* another bug fix to iterate camera topic name

* added a test for custom camera executor

* temporarily removing camera node because it's broken

* added camera node back

* changed executor publisher name because name conflict with roscamera

* something is wrong with publisher name. removed executor test from launch file

* Implemented camera toggle control via a mission control topic called 'camera_control' that sends string messages

* delete camera_subscriber (unused)

* rebase + updated roscamera.py with on, off functionality instead of toggle and added better documentation, fixed formatting

* update settings.json

* Robot Interface Merge (#24)

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* change viator launch and cooresponding package xml

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* change viator launch and cooresponding package xml

* added all methods to robot interface

* Added arm publishers and renamed other publishers

* Followed publisher naming standard

* update robot interface

* Fix RobotInterface import in testing node

* change message values to float 32

* Initial testing works. Tested three methods. Multiple motors move

* Changed const variables, changed math and gear ratios

* Can2.0 works

* Added some comments

* Added some comments

* Added setupCAN.sh

* Position code works with a single motor. Idk why the magic number is 33 for 2pi

---------

Co-authored-by: matthewperrywa <matthew.c.perry@outlook.com>
Co-authored-by: mendomic <mendomic@uw.edu>
Co-authored-by: Vladimir Kupryukhin <vladimirkupryukhin@gmail.com>
Co-authored-by: agrach <agrach@uw.edu>
Co-authored-by: VladimirKupryukhin <vlad@vlad-laptop>

* Merge RobotInfo (#29)

* Add RobotInfo

* Add ability to convert callback data

* [robot_info] add dummy nodes for testing

* [robot_info] fix topic name being grabbed incorrectly

* Edits for robot info

* Add RobotInfo

* Add ability to convert callback data

* [robot_info] add dummy nodes for testing

* [robot_info] fix topic name being grabbed incorrectly

* Edits for robot info

---------

Co-authored-by: TrickFire Robotics <trickfirerobotics@gmail.com>

* Merge drivebase code (#30)

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Initial drivebase, update docker & settings

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* instances robot interface

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* importing interface works

* change viator launch and cooresponding package xml

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* change viator launch and cooresponding package xml

* update topic/method names

* added all methods to robot interface

* copy interface to drivebase

* fix variable bug

* Minor fixes

* Add speed const & package description

* Convert functions to camelCase

* add drivebase to viator launch

* basic turn left and right

* Getting started

* Code overview

* Add image

* Added simple moteus docs

* First moteus docs draft

* Clean up moteus docs

* Rename moteus_can.md to can_moteus.md

* Spelling

* Edit getting started documents

* Fix gettingstarted link

* Added Git section to getting started

* Added suggestions from #17

* Add doc for CAN ID values (#19)

Added table of what CAN ID values we use for different subsystems.

* Update getting_started.md "ROS2"

* Create installing_on_mac.md

* Update installing_on_mac.md

* Update installing_on_mac.md

fix numbering

* Robot Interface Merge (#24)

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* change viator launch and cooresponding package xml

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* change viator launch and cooresponding package xml

* added all methods to robot interface

* Added arm publishers and renamed other publishers

* Followed publisher naming standard

* update robot interface

* Fix RobotInterface import in testing node

* change message values to float 32

* Initial testing works. Tested three methods. Multiple motors move

* Changed const variables, changed math and gear ratios

* Can2.0 works

* Added some comments

* Added some comments

* Added setupCAN.sh

* Position code works with a single motor. Idk why the magic number is 33 for 2pi

---------

Co-authored-by: matthewperrywa <matthew.c.perry@outlook.com>
Co-authored-by: mendomic <mendomic@uw.edu>
Co-authored-by: Vladimir Kupryukhin <vladimirkupryukhin@gmail.com>
Co-authored-by: agrach <agrach@uw.edu>
Co-authored-by: VladimirKupryukhin <vlad@vlad-laptop>

* First commit, testing

* Created robot_interface.cpp file

* change viator launch and cooresponding package xml

* Remove stuff to match main

* Properly import robot interface

* Delete unneeded stuff

* Revert "Delete unneeded stuff"

This reverts commit c6ac076.

* Fix message type

* Fix stuff

* Fix func names and variables

* Fix motor

* Idk what 70-fdcanusb.rules is

* Fix type errors from messages

* Both sides of the drivebase spin

* Initial drivebase, update docker & settings

* instances robot interface

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* importing interface works

* update topic/method names

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* change viator launch and cooresponding package xml

* change viator launch and cooresponding package xml

* added all methods to robot interface

* copy interface to drivebase

* fix variable bug

* Minor fixes

* Add speed const & package description

* Convert functions to camelCase

* add drivebase to viator launch

* basic turn left and right

* Getting started

* Code overview

* Added simple moteus docs

* First moteus docs draft

* Clean up moteus docs

* Rename moteus_can.md to can_moteus.md

* Spelling

* Edit getting started documents

* Fix gettingstarted link

* Added Git section to getting started

* Added suggestions from #17

* Add doc for CAN ID values (#19)

Added table of what CAN ID values we use for different subsystems.

* Update getting_started.md "ROS2"

* Create installing_on_mac.md

* Update installing_on_mac.md

* Update installing_on_mac.md

fix numbering

* Robot Interface Merge (#24)

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* change viator launch and cooresponding package xml

* First commit, added some C++ support

* First commit, testing

* Added robot_interface.h file

* Started working on adding function prototypes to robot_interface.h

* update header for robot interface

* Created robot_interface.cpp file

* add antenna methods

* Started converting robot_interface to Python

* Added constructor/destructor

* Trying to experiment with publisher

* Trying to create testing node for publishing testing

* Random testing node testing

* Got some testing publishers to publish using smaller node. Not sure if it's working correctly

* Added some more testing publisher stuff for all 6 wheels. Removed forward/backward wheel functionality, only single wheel function (positive/negative)

* Changed conversion from using diameter to using circumference

* change viator launch and cooresponding package xml

* added all methods to robot interface

* Added arm publishers and renamed other publishers

* Followed publisher naming standard

* update robot interface

* Fix RobotInterface import in testing node

* change message values to float 32

* Initial testing works. Tested three methods. Multiple motors move

* Changed const variables, changed math and gear ratios

* Can2.0 works

* Added some comments

* Added some comments

* Added setupCAN.sh

* Position code works with a single motor. Idk why the magic number is 33 for 2pi

---------

Co-authored-by: matthewperrywa <matthew.c.perry@outlook.com>
Co-authored-by: mendomic <mendomic@uw.edu>
Co-authored-by: Vladimir Kupryukhin <vladimirkupryukhin@gmail.com>
Co-authored-by: agrach <agrach@uw.edu>
Co-authored-by: VladimirKupryukhin <vlad@vlad-laptop>

* First commit, testing

* Created robot_interface.cpp file

* Remove stuff to match main

* Properly import robot interface

* Delete unneeded stuff

* Revert "Delete unneeded stuff"

This reverts commit c6ac076.

* Fix message type

* Fix type errors from messages

* Both sides of the drivebase spin

* made the drivebase work

* Removed the print statements that flood the console

* launch_only_docker.sh runs launch.sh automatically

* launch_only_docker.sh opens in project folder, doesn't run launch.sh

* Remove unneeded python linting setting

* Change version to 0.0.0

* Update description

---------

Co-authored-by: Daniel K <daniel.kantor2009@gmail.com>
Co-authored-by: matthewperrywa <matthew.c.perry@outlook.com>
Co-authored-by: Vladimir Kupryukhin <vladimirkupryukhin@gmail.com>
Co-authored-by: Ezekiel Mitchell <135088287+ezekielmitchell@users.noreply.github.com>
Co-authored-by: mendomic <mendomic@uw.edu>
Co-authored-by: agrach <agrach@uw.edu>
Co-authored-by: VladimirKupryukhin <vlad@vlad-laptop>
Co-authored-by: TrickFire Robotics <trickfirerobotics@gmail.com>

* Rewrite can_moteus (#41)

* Basic node works

* Ready to test code

* Add launch file modifications

* Trying to connect to moteus works

* Wheel spins via the robot interface using json

* Motors stop after termination

* Updated interface to support drivebase

* Remove old files. Add shutting motor down when error found

* Removed repeated code

* Tested stuff

* Works

* testing canbus disconnection

* Canbus disconnection works

* Relaunching now works without unplugging.replugging canusb

* Added some comments

* Motor reconnect works

* Proper handling of faults

* Handle exception when shutting down. Checks for invalid ros context

* Updated RobotInfo

* Removed unneeded stuff from the json helpers

* Added comments and a few constants

* Docs draft 1

* Updated error messages to be more detailed

* Some more can_moteus.md docs

* Finish documentation

* can_modeus.md minor grammar changes

---------

Co-authored-by: TrickFire Robotics <trickfirerobotics@gmail.com>
Co-authored-by: Carl Klay <klaymorer@protonmail.com>

* Send Telemetry Data to Mission Control (#53)

* Basic node setup

* Need to test publisher code

* Send data to mission control

* READING POWER WORKS WOOO

* Make moteus send telemetry even when set_stop()

---------

Co-authored-by: TrickFire Robotics <trickfirerobotics@gmail.com>

* arm: Implement basic arm functionality (velocity-based) with an xbox controller (#55)

* Basic arm package works. Handle canusb not being plugged in

* Basic service works

* Change roverROS armstate from mission control

* Added unveiling-arm topics for the motors. Need to implement the robotinterface

* Updated interface for arm. Untested

* Prevent individual control vel from sending when it is disabled

* Remove some bad code due to me copy/pasting

* Tested. Working

* Last commit before merge

---------

Co-authored-by: TrickFire Robotics <trickfirerobotics@gmail.com>

* Remove unused packages, remove unused docs, remove unused shell files

* [reformat] remove existing linter tests

these tests are nigh impossible to integrate with and customize, plus it would be a pain to integrate pylint support. plus these tests don't run on lib files

* THE GREAT REFORMATTENING

there's a long laundry list of changes (let's hope i don't forget anything):
- added pylint, isort, black, and mypy
  - pylint does most of the linting work
  - isort is specifically for import sorting, and some whitespace issues
  - black runs autoformatting on save
    - I don't particularly like autoformatters but I know for a fact that people will not heed the warnings for linters so ive included it as an
extreme measure.
  - mypy is for type checking
- linting has several things people should pay attention to
  - max line length is 100
  - doc strings for all functions, classes, and modules is required
  - imports now must be in specific orders
    - isort will automatically sort imports for you
  - function parameters must be type hinted
  - function return types must be specified too
  - some class attributes must also be type hinted
  - function names will be camelCase
  - attributes, variables, and file names must be snake_case
  - whitespace will be automatically managed by the auto formatter
- other than consistent formatting, this will give type hinting and intellisense to almost every variable

* THE GREAT REFORMATTENING (pt 2)

* [lib] Refactor non-node modules to be easier to work with (#59)

* [lib] add lib folder and refactor constants into configs, removing motor names only relying on can id

* [lib] refactor robot interface and info to take motor configs and new msg types

* [lib] make coloring slightly less annoying

* [lib] differentiate between disable and stop motor

* [lib] set position to `nan` when stopping motor

* add typing to moteus command results

* [lib] debug in person to make it work

* [lib] make robot info take motor configs over can ids

* [lib] fix formatting

* fix units on robot interface and document some motor state attributes

* [lib] Changed drivebase speed to match radian/s inputs rather than rev/s

---------

Co-authored-by: TrickFire Robotics <trickfirerobotics@gmail.com>

* [lib] Add support for setting moteus config values in lib #61  (#62)

* [lib] add lib folder and refactor constants into configs, removing motor names only relying on can id

* [lib] refactor robot interface and info to take motor configs and new msg types

* [lib] make coloring slightly less annoying

* [lib] differentiate between disable and stop motor

* [lib] set position to `nan` when stopping motor

* add typing to moteus command results

* [lib] debug in person to make it work

* [lib] make robot info take motor configs over can ids

* [lib] fix formatting

* fix units on robot interface and document some motor state attributes

* [lib] implement config setting

* [lib] add configs

* [lib] remove conf default and fix bugs for config

* [lib] skip can id config value

---------

Co-authored-by: Kyler Li <loukylor@gmail.com>
Co-authored-by: TrickFire Robotics <trickfirerobotics@gmail.com>

* [launch] Make a shell script to launch, run, build, create docker images/containers

* [launch] added -t for tty

* [launch] Added some comments to container_launch.sh

* Automatically stop the old container from running

* Adds an Example Node as well as Some Additional Documentation (#63)

* [example/docs] Remade coder_overview.md

* [example/docs] Added basic example node that has hello world in it

* Added example node with timer, pub, sub

* Spelling fixes for docs

* Removed example_node from the node launch array

* [example/docs] Small .md formatting changes and sentence restructure

* [example/lib] Update main README.md

* Formatted the example.py

* [example/docs] add building and testing docs

---------

Co-authored-by: loukylor <loukylor@gmail.com>

* [doc/example] remove example node from viator lauch (#65)

* Hong/heartbeat (#70)

* heartbeat: finished heartbeat code [untested]

* heartbeat: remove unnecessary file

* heartbeat: change initialization wait time from 3s to 10s

* heartbeat: tested [on 10/1]

* added the missing Docker modules  in the Dockerfile for OpenCV and cv_bridge

* Final Luncheon code

* Cleanup

* Further cleanup

* Update workflow to work with submodules

* Fix workflow erroring on vision_opencv

* Fix pylint

* Remove old camera on/off functionality

* Fix video permission issue

---------

Co-authored-by: Vladimir Kupryukhin <vladimirkupryukhin@gmail.com>
Co-authored-by: VladimirKupryukhin <vlad@vlad-laptop>
Co-authored-by: Razvan Anghel <csneed@Razvans-MacBook-Air.local>
Co-authored-by: TrickFire Robotics <trickfirerobotics@gmail.com>
Co-authored-by: dkim03 <kimdavid2222@gmail.com>
Co-authored-by: Daniel Kantor <daniel.kantor2009@gmail.com>
Co-authored-by: matthewperrywa <matthew.c.perry@outlook.com>
Co-authored-by: mendomic <mendomic@uw.edu>
Co-authored-by: agrach <agrach@uw.edu>
Co-authored-by: Kyler Li <46853114+loukylor@users.noreply.github.com>
Co-authored-by: Carl Klay <klaymorer@protonmail.com>
Co-authored-by: Ezekiel Mitchell <135088287+ezekielmitchell@users.noreply.github.com>
Co-authored-by: Vladimir Kupryukhin <45701631+VladimirKupryukhin@users.noreply.github.com>
Co-authored-by: Kyler Li <loukylor@gmail.com>
Co-authored-by: Adam Chhor <chhoradam@gmail.com>
  • Loading branch information
16 people authored Jan 18, 2025
1 parent 3b26192 commit 9a524d3
Show file tree
Hide file tree
Showing 14 changed files with 208 additions and 6 deletions.
14 changes: 12 additions & 2 deletions .devcontainer/Dockerfile
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
# Base image: Ubuntu 20.04 with ROS2 Humble installed
FROM ros:humble

# Install dependencies from apt
RUN apt-get update && apt-get install -y \
# To use git in the container
Expand All @@ -15,6 +14,12 @@ RUN apt-get update && apt-get install -y \
can-utils \
iproute2 \
kmod \
# Used for cv_bridge
python3-numpy \
libboost-python-dev \
# OpenCV
libopencv-dev \
python3-opencv \
# Clean out the apt lists after `apt-get update`
&& rm -rf /var/lib/apt/lists/*

Expand All @@ -24,6 +29,9 @@ RUN pip3 install moteus

RUN pip3 install pyusb

# Install OpenCV for python
RUN pip3 install opencv-python

# Update pydocstyle to remove a deprecation warning when testing for PEP257
RUN pip install --upgrade pydocstyle

Expand All @@ -34,7 +42,9 @@ RUN useradd trickfire \
# Give it a directory in /home
--create-home \
# Don't make a giant log file for login data, we don't care about it
--no-log-init
--no-log-init \
# Allow the user to access video cameras.
-G video

# Copy all the bash customizations over to the user.
COPY .devcontainer/trickfire.bashrc /home/trickfire/.bashrc
2 changes: 1 addition & 1 deletion .devcontainer/devcontainer.json
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
"workspaceFolder": "/home/trickfire/${localWorkspaceFolderBasename}",
"runArgs": [
"--cap-add=SYS_PTRACE",
//"--device=/dev/ttyUSB0",
"--device=/dev/video0",
"--security-opt", "seccomp=unconfined"
],
"customizations": {
Expand Down
8 changes: 5 additions & 3 deletions .github/workflows/lint.yml
Original file line number Diff line number Diff line change
Expand Up @@ -7,12 +7,14 @@ jobs:
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v4
with:
submodules: true
# Black has a first-party ci/cd solution doesn't require python or setup, so use that
# before running setup.
- name: Run Black
uses: psf/black@stable
with:
options: "--check --verbose"
options: "--check --verbose --exclude vision_opencv"
src: "./src"
version: 24.8.0
- name: Setup Python
Expand All @@ -26,7 +28,7 @@ jobs:
- name: Run Pylint
# Ignoring ros packages is unideal, but installing them as packages is not trivial
run: |
pylint --fail-under=8 --ignored-modules=rclpy,std_msgs,ament_copyright,ament_flake8,pytest,launch ./src
pylint --fail-under=8 --ignored-modules=rclpy,std_msgs,ament_copyright,ament_flake8,pytest,launch,vision_opencv --ignore-paths=^src/vision_opencv.*$ ./src
- name: Run Mypy
run: |
mypy --exclude /test/ --exclude setup\.py ./src
mypy --exclude /test/ --exclude setup\.py --exclude vision_opencv ./src
4 changes: 4 additions & 0 deletions .gitmodules
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
[submodule "src/vision_opencv"]
path = src/vision_opencv
url = https://github.com/ros-perception/vision_opencv.git
branch = ros2
Empty file added src/camera/camera/__init__.py
Empty file.
111 changes: 111 additions & 0 deletions src/camera/camera/roscamera.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,111 @@
import cv2 # OpenCV library
import rclpy # Python Client Library for ROS 2
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node # Handles the creation of nodes
from sensor_msgs.msg import CompressedImage # Image is the message type


def getCameras() -> list[int]:
"""
Function returns a list of working camera IDs to capture every camera connected to the robot
"""

non_working_ports = 0
dev_port = 0
working_ports = []

while non_working_ports < 6:
camera = cv2.VideoCapture(dev_port)
if not camera.isOpened():
non_working_ports += 1
else:
is_reading, img = camera.read()
_ = camera.get(3)
_ = camera.get(4)
if is_reading:
working_ports.append(dev_port)

dev_port += 1

return working_ports


class RosCamera(Node):

def __init__(self, topicName: str, camera: int):
super().__init__("ros_camera")
self.get_logger().info("Launching ros_camera node")

# Create the publisher. This publisher will publish an Image
# to the video_frames topic. The queue size is 10 messages.
self._publisher = self.create_publisher(CompressedImage, topicName, 10)
self.get_logger().info("Created Publisher " + topicName)

# We will publish a message every 0.1 seconds
# timer_period = 0.1 # seconds
timer_period = 0.1 # seconds

# Create the timer
self.timer = self.create_timer(timer_period, self.publishCameraFrame)

# Create a VideoCapture object
# The argument '0' gets the default webcam.
self.cap = cv2.VideoCapture(camera)
self.get_logger().info(
"Using video ID: " + str(camera) + ", ON: " + str(self.cap.isOpened())
)

# Used to convert between ROS and OpenCV images
self.br = CvBridge()

# Declare parameters for each 'ros_camera' node thread
self.declare_parameter("cameraNumber", camera)

def publishCameraFrame(self) -> None:
"""
Callback function publishes a frame captured from a camera to /video_framesX (X is specific
camera ID) every 0.1 seconds
"""

# Capture frame-by-frame
# This method returns True/False as well as the video frame.
ret, frame = self.cap.read()

if ret:
# Publish the image.
self._publisher.publish(self.br.cv2_to_compressed_imgmsg(frame))


def main(args: list[str] | None = None) -> None:
"""
The entry point of the node.
"""

rclpy.init(args=args)
try:
# We need an executor because running .spin() is a blocking function.
# using the MultiThreadedExecutor, we can control multiple nodes
executor = MultiThreadedExecutor()
nodes = []
camera_num = 0

for camera_id in getCameras():
node = RosCamera("video_frames" + str(camera_num), camera_id)
nodes.append(node)
executor.add_node(node)
camera_num += 1

try:
executor.spin()
finally:
executor.shutdown()
for node in nodes:
node.destroy_node()

finally:
rclpy.shutdown()


if __name__ == "__main__":
main()
18 changes: 18 additions & 0 deletions src/camera/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>camera</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="kimdavid2222@gmail.com">trickfire</maintainer>
<license>TODO: License declaration</license>

<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>

<export>
<build_type>ament_python</build_type>
</export>
</package>
Empty file added src/camera/resource/camera
Empty file.
4 changes: 4 additions & 0 deletions src/camera/setup.cfg
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
[develop]
script_dir=$base/lib/camera
[install]
install_scripts=$base/lib/camera
23 changes: 23 additions & 0 deletions src/camera/setup.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,23 @@
from setuptools import find_packages, setup

package_name = "camera"

setup(
name=package_name,
version="0.0.0",
packages=find_packages(exclude=["test"]),
data_files=[
("share/ament_index/resource_index/packages", ["resource/" + package_name]),
("share/" + package_name, ["package.xml"]),
],
install_requires=["setuptools"],
zip_safe=True,
maintainer="trickfire",
maintainer_email="kimdavid2222@gmail.com",
description="TODO: Package description",
license="TODO: License declaration",
tests_require=["pytest"],
entry_points={
"console_scripts": ["roscamera = camera.roscamera:main"],
},
)
25 changes: 25 additions & 0 deletions src/camera/test/test_copyright.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

import pytest
from ament_copyright.main import main


# Remove the `skip` decorator once the source file(s) have a copyright header
@pytest.mark.skip(reason="No copyright header has been placed in the generated source file.")
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=[".", "test"])
assert rc == 0, "Found errors"
3 changes: 3 additions & 0 deletions src/viator_launch/launch/robot.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@

heartbeat_node = Node(package="heartbeat", executable="heartbeat", name="heartbeat_node")

camera_node = Node(package="camera", executable="roscamera", name="camera_node")

# This is the example node. It will show ROS timers, subscribers, and publishers
# To include it in the startup, add it to the array in the generate_launch_description() method
example_node = Node(package="example_node", executable="myExampleNode", name="my_example_node")
Expand All @@ -44,6 +46,7 @@ def generate_launch_description() -> launch.LaunchDescription: # pylint: disabl
mission_control_updater_node,
arm_node,
heartbeat_node,
camera_node,
launch_include,
]
)
1 change: 1 addition & 0 deletions src/viator_launch/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
<exec_depend>ros2launch</exec_depend>
<exec_depend>mission_control_updater</exec_depend>
<exec_depend>arm</exec_depend>
<exec_depend>camera</exec_depend>
<exec_depend>example_node</exec_depend>


Expand Down
1 change: 1 addition & 0 deletions src/vision_opencv
Submodule vision_opencv added at a34a0c

0 comments on commit 9a524d3

Please sign in to comment.