BRSRI/bellande_humanoid_robot_intelligence_control_system_framework
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge pull request #22 from RonaldsonBellande/main

Browse Source 
seperate
This commit is contained in:
Ronaldson Bellande
2024-07-26 17:19:14 -04:00
committed by GitHub
parent bb68e1519a eb41a97d90
commit 0c0679a668
28 changed files with 1353 additions and 3180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
.travis.yml
View File

@@ -1,30 +0,0 @@
# This config file for Travis CI utilizes ros-industrial/industrial_ci package.
# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst
sudo: required
dist: trusty
services:
- docker
language: generic
python:
- "3.8"
compiler:
- gcc
notifications:
email:
on_success: change
on_failure: always
recipients:
- ronaldsonbellande@gmail.com
env:
matrix:
- ROS_DISTRO=noetic ROS_REPO=ros-shadow-fixed UPSTREAM_WORKSPACE=file OS_NAME=ubuntu OS_CODE_NAME=focal $ROSINSTALL_FILENAME=".humanoid_robot_intelligence_control_system_framework.rosinstall"
branches:
only:
- master
- noetic
install:
- git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
script:
- source .ci_config/travis.sh

136
humanoid_robot_intelligence_control_system_controller/CHANGELOG.rst
View File

@@ -1,136 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package humanoid_robot_intelligence_control_system_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.2 (2023-10-03)
------------------
* Make it compatible for ROS1/ROS2
* Fix bugs
* Update package.xml and CMakeList.txt for to the latest versions
* Contributors & Maintainer: Ronaldson Bellande
0.3.1 (2023-09-27)
------------------
* Starting from this point it under a new license
* Fix errors and Issues
* Rename Repository for a completely different purpose
* Make it compatible with ROS/ROS2
* Upgrade version of all builds and make it more compatible
* Update package.xml and CMakeList.txt for to the latest versions
* Contributors & Maintainer: Ronaldson Bellande
0.3.0 (2021-05-03)
------------------
* Update package.xml and CMakeList.txt for noetic branch
* Contributors: Ronaldson Bellande
0.2.9 (2018-03-22)
------------------
* added serivce for setting module
* deleted comment for debug
* modified to prevent duplicate indirect address write
* added boost system dependencies
* fixed a bug that occure when handling bulk read item that does not exist.
* Contributors: Kayman, Zerom, Pyo
0.2.8 (2018-03-20)
------------------
* modified CMakeLists.txt for system dependencies (yaml-cpp)
* Contributors: Zerom, Pyo
0.2.7 (2018-03-15)
------------------
* changed the License and package format to version 2
* changed all values read by bulk read are saved to dxl_state\_->bulk_read_table\_.
* Modified to prevent duplicate indirect address write
* Contributors: SCH, Zerom, Pyo
0.2.6 (2017-08-09)
------------------
* multi thread bug fixed.
* unnecessary debug print removed.
* OpenCR control table item name changed. (torque_enable -> dynamixel_power)
* fixed to not update update_time_stamp\_ if bulk read fails.
* Contributors: Zerom
0.2.5 (2017-06-09)
------------------
* updated for yaml-cpp dependencies
* Contributors: SCH
0.2.4 (2017-06-07)
------------------
* added cmake_modules in package.xml
* Contributors: SCH
0.2.3 (2017-05-23)
------------------
* updated the cmake file for ros install
* Contributors: SCH
0.2.2 (2017-04-24)
------------------
* updated humanoid_robot_intelligence_control_system_controller.cpp
* changed to read control cycle from .robot file
* Contributors: Zerom
0.2.1 (2016-11-23)
------------------
* Merge the changes and update
* - Direct Control Mode bug fixed.
* update
* - added writeControlTableCallback
* - added WriteControlTable msg callback
* mode change debugging
* - optimized cpu usage by spin loop (by astumpf)
* - humanoid_robot_intelligence_control_system_controller process() : processing order changed.
* 1st : packet communication
* 2nd : processing modules
* - dependencies fixed. (Pull requests `#26 <https://github.com/ROBOTIS-GIT/ROBOTIS-Framework/issues/26>`_)
* - make setJointCtrlModuleCallback() to the thread function & improved.
* - modified dependency problem.
* - reduce CPU consumption
* Contributors: Jay Song, Pyo, Zerom, SCH
0.2.0 (2016-08-31)
------------------
* bug fixed (position pid gain & velocity pid gain sync write).
* added velocity_to_value_ratio to DXL Pro-H series.
* changed some debug messages.
* added velocity p/i/d gain and position i/d gain sync_write code.
* SyncWriteItem bug fixed.
* add function / modified the code simple (using auto / range based for loop)
* added XM-430-W210 / XM-430-W350 device file.
* rename ControlMode(CurrentControl -> TorqueControl)
* rename (port_to_sync_write_torque\_ -> port_to_sync_write_current\_)
* rename (present_current\_ -> present_torque\_)
* modified torque control code
* fixed typos / changed ROS_INFO -> fprintf (for processing speed)
* startTimer() : after bulkread txpacket(), need some sleep()
* changed the order of processing in the Process() function.
* added missing mutex for gazebo
* fixed crash when running in gazebo simulation
* sync write bug fix.
* added position_p_gain sync write
* MotionModule/SensorModule member variable access changed (public -> protected).
* Contributors: Jay Song, Zerom, Pyo, SCH
0.1.1 (2016-08-18)
------------------
* updated the package information
0.1.0 (2016-08-12)
------------------
* first public release for Kinetic
* modified the package information for release
* develop branch -> master branch
* function name changed : DeviceInit() -> InitDevice()
* Fixed high CPU consumption due to busy waits
* add SensorState
add Singleton template
* XM-430 / CM-740 device file added.
Sensor device added.
* added code to support the gazebo simulator
* added first bulk read failure protection code
* renewal
* Contributors: Alexander Stumpf, Jay Song, Zerom, Pyo

157
humanoid_robot_intelligence_control_system_controller/CMakeLists.txt Executable file → Normal file
View File

@@ -1,97 +1,60 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# 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.
cmake_minimum_required(VERSION 3.8)
project(humanoid_robot_intelligence_control_system_controller)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_intelligence_control_system_controller_msgs
dynamixel_sdk
humanoid_robot_intelligence_control_system_device
humanoid_robot_intelligence_control_system_framework_common
cmake_modules
)
find_package(Boost REQUIRED)
find_package(PkgConfig REQUIRED)
else()
find_package(ament_cmake REQUIRED)
endif()
pkg_check_modules(YAML_CPP REQUIRED yaml-cpp)
find_path(YAML_CPP_INCLUDE_DIR
NAMES yaml_cpp.h
PATHS ${YAML_CPP_INCLUDE_DIRS}
)
find_library(YAML_CPP_LIBRARY
NAMES YAML_CPP
PATHS ${YAML_CPP_LIBRARY_DIRS}
)
link_directories(${YAML_CPP_LIBRARY_DIRS})
if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
add_definitions(-DHAVE_NEW_YAMLCPP)
endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5")
if($ENV{ROS_VERSION} EQUAL 1)
catkin_package(
INCLUDE_DIRS include
LIBRARIES humanoid_robot_intelligence_control_system_controller
CATKIN_DEPENDS
roscpp
roslib
std_msgs
sensor_msgs
humanoid_robot_intelligence_control_system_controller_msgs
dynamixel_sdk
humanoid_robot_intelligence_control_system_device
humanoid_robot_intelligence_control_system_framework_common
cmake_modules
DEPENDS Boost
)
endif()
include_directories(
include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
${YAML_CPP_INCLUDE_DIRS}
)
add_library(humanoid_robot_intelligence_control_system_controller src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp)
add_dependencies(humanoid_robot_intelligence_control_system_controller ${catkin_EXPORTED_TARGETS})
target_link_libraries(humanoid_robot_intelligence_control_system_controller ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${YAML_CPP_LIBRARIES})
install(TARGETS humanoid_robot_intelligence_control_system_controller
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# 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.
cmake_minimum_required(VERSION 3.0.2)
project(humanoid_robot_intelligence_control_system_controller)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
rospy
)
catkin_package(
CATKIN_DEPENDS
rospy
)
else()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
endif()
if($ENV{ROS_VERSION} EQUAL 1)
catkin_python_setup()
catkin_install_python(PROGRAMS
src/PIDController.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY config launch rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
else()
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
src/PIDController.py
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY config launch rviz
DESTINATION share/${PROJECT_NAME}
)
ament_package()
endif()

209
humanoid_robot_intelligence_control_system_controller/include/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.h
View File

@@ -1,209 +0,0 @@
/*******************************************************************************
* Copyright 2018 ROBOTIS CO., LTD.
*
* 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.
*******************************************************************************/
/*
* humanoid_robot_intelligence_control_system_controller.h
*
* Created on: 2016. 1. 15.
* Author: zerom
*/
#ifndef ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#define ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#include "humanoid_robot_intelligence_control_system_controller_msgs/GetJointModule.h"
#include "humanoid_robot_intelligence_control_system_controller_msgs/JointCtrlModule.h"
#include "humanoid_robot_intelligence_control_system_controller_msgs/LoadOffset.h"
#include "humanoid_robot_intelligence_control_system_controller_msgs/SetJointModule.h"
#include "humanoid_robot_intelligence_control_system_controller_msgs/SetModule.h"
#include "humanoid_robot_intelligence_control_system_controller_msgs/SyncWriteItem.h"
#include "humanoid_robot_intelligence_control_system_controller_msgs/WriteControlTable.h"
#include "dynamixel_sdk/group_bulk_read.h"
#include "dynamixel_sdk/group_sync_write.h"
#include "humanoid_robot_intelligence_control_system_device/robot.h"
#include "humanoid_robot_intelligence_control_system_framework_common/motion_module.h"
#include "humanoid_robot_intelligence_control_system_framework_common/sensor_module.h"
namespace humanoid_robot_intelligence_control_system_framework {
enum ControllerMode { MotionModuleMode, DirectControlMode };
class RobotisController : public Singleton<RobotisController> {
private:
boost::thread queue_thread_;
boost::thread gazebo_thread_;
boost::thread set_module_thread_;
boost::mutex queue_mutex_;
bool init_pose_loaded_;
bool is_timer_running_;
bool is_offset_enabled_;
double offset_ratio_;
bool stop_timer_;
pthread_t timer_thread_;
ControllerMode controller_mode_;
std::list<MotionModule *> motion_modules_;
std::list<SensorModule *> sensor_modules_;
std::vector<dynamixel::GroupSyncWrite *> direct_sync_write_;
std::map<std::string, double> sensor_result_;
void gazeboTimerThread();
void msgQueueThread();
void setCtrlModuleThread(std::string ctrl_module);
void setJointCtrlModuleThread(
const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg);
bool isTimerStopped();
void initializeSyncWrite();
public:
const int NONE_GAIN = 65535;
bool DEBUG_PRINT;
Robot *robot_;
bool gazebo_mode_;
std::string gazebo_robot_name_;
/* bulk read */
std::map<std::string, dynamixel::GroupBulkRead *> port_to_bulk_read_;
/* sync write */
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_current_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_p_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_i_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_position_d_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_p_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_i_gain_;
std::map<std::string, dynamixel::GroupSyncWrite *>
port_to_sync_write_velocity_d_gain_;
/* publisher */
ros::Publisher goal_joint_state_pub_;
ros::Publisher present_joint_state_pub_;
ros::Publisher current_module_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_position_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_velocity_pub_;
std::map<std::string, ros::Publisher> gazebo_joint_effort_pub_;
static void *timerThread(void *param);
RobotisController();
bool initialize(const std::string robot_file_path,
const std::string init_file_path);
void initializeDevice(const std::string init_file_path);
void process();
void addMotionModule(MotionModule *module);
void removeMotionModule(MotionModule *module);
void addSensorModule(SensorModule *module);
void removeSensorModule(SensorModule *module);
void startTimer();
void stopTimer();
bool isTimerRunning();
void setCtrlModule(std::string module_name);
void loadOffset(const std::string path);
/* ROS Topic Callback Functions */
void writeControlTableCallback(
const humanoid_robot_intelligence_control_system_controller_msgs::WriteControlTable::ConstPtr &msg);
void syncWriteItemCallback(
const humanoid_robot_intelligence_control_system_controller_msgs::SyncWriteItem::ConstPtr &msg);
void setControllerModeCallback(const std_msgs::String::ConstPtr &msg);
void setJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
void setJointCtrlModuleCallback(
const humanoid_robot_intelligence_control_system_controller_msgs::JointCtrlModule::ConstPtr &msg);
void setCtrlModuleCallback(const std_msgs::String::ConstPtr &msg);
void enableOffsetCallback(const std_msgs::Bool::ConstPtr &msg);
bool getJointCtrlModuleService(
humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Request &req,
humanoid_robot_intelligence_control_system_controller_msgs::GetJointModule::Response &res);
bool setJointCtrlModuleService(
humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Request &req,
humanoid_robot_intelligence_control_system_controller_msgs::SetJointModule::Response &res);
bool setCtrlModuleService(
humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Request &req,
humanoid_robot_intelligence_control_system_controller_msgs::SetModule::Response &res);
bool
loadOffsetService(humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Request &req,
humanoid_robot_intelligence_control_system_controller_msgs::LoadOffset::Response &res);
void gazeboJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg);
int ping(const std::string joint_name, uint8_t *error = 0);
int ping(const std::string joint_name, uint16_t *model_number,
uint8_t *error = 0);
int action(const std::string joint_name);
int reboot(const std::string joint_name, uint8_t *error = 0);
int factoryReset(const std::string joint_name, uint8_t option = 0,
uint8_t *error = 0);
int read(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
int readCtrlItem(const std::string joint_name, const std::string item_name,
uint32_t *data, uint8_t *error = 0);
int read1Byte(const std::string joint_name, uint16_t address, uint8_t *data,
uint8_t *error = 0);
int read2Byte(const std::string joint_name, uint16_t address, uint16_t *data,
uint8_t *error = 0);
int read4Byte(const std::string joint_name, uint16_t address, uint32_t *data,
uint8_t *error = 0);
int write(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
int writeCtrlItem(const std::string joint_name, const std::string item_name,
uint32_t data, uint8_t *error = 0);
int write1Byte(const std::string joint_name, uint16_t address, uint8_t data,
uint8_t *error = 0);
int write2Byte(const std::string joint_name, uint16_t address, uint16_t data,
uint8_t *error = 0);
int write4Byte(const std::string joint_name, uint16_t address, uint32_t data,
uint8_t *error = 0);
int regWrite(const std::string joint_name, uint16_t address, uint16_t length,
uint8_t *data, uint8_t *error = 0);
};
} // namespace humanoid_robot_intelligence_control_system_framework
#endif /* ROBOTIS_CONTROLLER_ROBOTIS_CONTROLLER_H_ */

100
humanoid_robot_intelligence_control_system_controller/package.xml Executable file → Normal file
View File

@@ -17,96 +17,30 @@ the License.
<package format="3">
<name>humanoid_robot_intelligence_control_system_controller</name>
<version>0.3.2</version>
<description>humanoid_robot_intelligence_control_system_controller package for ROBOTIS's platform
like Manipulator-H, THORMANG and OP series</description>
<version>0.0.1</version>
<description>
This Package is for Detection Math
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<build_depend condition="$ROS_VERSION == 1">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 1">roslib</build_depend>
<build_depend condition="$ROS_VERSION == 1">std_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">sensor_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 1">dynamixel_sdk</build_depend>
<build_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_device</build_depend>
<build_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_framework_common</build_depend>
<build_depend condition="$ROS_VERSION == 1">cmake_modules</build_depend>
<build_depend condition="$ROS_VERSION == 1">yaml-cpp</build_depend>
<build_export_depend condition="$ROS_VERSION == 1">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">roslib</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">std_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">sensor_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">dynamixel_sdk</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_device</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_framework_common</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">cmake_modules</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 1">yaml-cpp</build_export_depend>
<exec_depend condition="$ROS_VERSION == 1">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">roslib</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">std_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_controller_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">dynamixel_sdk</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">humanoid_robot_intelligence_control_system_device</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">
humanoid_robot_intelligence_control_system_framework_common</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">cmake_modules</exec_depend>
<exec_depend condition="$ROS_VERSION == 1">yaml-cpp</exec_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<build_depend condition="$ROS_VERSION == 2">roscpp</build_depend>
<build_depend condition="$ROS_VERSION == 2">roslib</build_depend>
<build_depend condition="$ROS_VERSION == 2">std_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">sensor_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</build_depend>
<build_depend condition="$ROS_VERSION == 2">dynamixel_sdk</build_depend>
<build_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_device</build_depend>
<build_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_framework_common</build_depend>
<build_depend condition="$ROS_VERSION == 2">cmake_modules</build_depend>
<build_depend condition="$ROS_VERSION == 2">yaml-cpp</build_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<build_export_depend condition="$ROS_VERSION == 2">roscpp</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">roslib</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">std_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">sensor_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">dynamixel_sdk</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_device</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_framework_common</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">cmake_modules</build_export_depend>
<build_export_depend condition="$ROS_VERSION == 2">yaml-cpp</build_export_depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<exec_depend condition="$ROS_VERSION == 2">roscpp</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">roslib</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">std_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">sensor_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_controller_msgs</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">dynamixel_sdk</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">humanoid_robot_intelligence_control_system_device</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">
humanoid_robot_intelligence_control_system_framework_common</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">cmake_modules</exec_depend>
<exec_depend condition="$ROS_VERSION == 2">yaml-cpp</exec_depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_controller/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/PIDController.py'],
packages=['humanoid_robot_intelligence_control_system_controller'],
package_dir={'': 'src'},
)
setup(**setup_args)

50
humanoid_robot_intelligence_control_system_controller/src/PIDController.py Normal file
View File

@@ -0,0 +1,50 @@
#!/usr/bin/env python3
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import time
from typing import Dict, List, Tuple
class PIDController:
def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))):
self.kp, self.ki, self.kd = gains
self.name = name
self.output_limits = output_limits
self.reset()
def reset(self):
self.last_error = 0
self.integral = 0
self.last_time = time.time()
def compute(self, setpoint: float, process_variable: float) -> float:
current_time = time.time()
dt = current_time - self.last_time
error = setpoint - process_variable
self.integral += error * dt
derivative = (error - self.last_error) / dt if dt > 0 else 0
output = self.kp * error + self.ki * self.integral + self.kd * derivative
output = max(min(output, self.output_limits[1]), self.output_limits[0])
self.last_error = error
self.last_time = current_time
return output
def update_config(self, gains: Tuple[float, float, float]):
self.kp, self.ki, self.kd = gains

2625
humanoid_robot_intelligence_control_system_controller/src/humanoid_robot_intelligence_control_system_controller/humanoid_robot_intelligence_control_system_controller.cpp
View File

File diff suppressed because it is too large Load Diff

60
humanoid_robot_intelligence_control_system_external_sensors/CMakeLists.txt Normal file
View File

@@ -0,0 +1,60 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# 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.
cmake_minimum_required(VERSION 3.0.2)
project(humanoid_robot_intelligence_control_system_controller)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
rospy
)
catkin_package(
CATKIN_DEPENDS
rospy
)
else()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
endif()
if($ENV{ROS_VERSION} EQUAL 1)
catkin_python_setup()
catkin_install_python(PROGRAMS
src/PIDController.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY config launch rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
else()
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
src/PIDController.py
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY config launch rviz
DESTINATION share/${PROJECT_NAME}
)
ament_package()
endif()

46
humanoid_robot_intelligence_control_system_external_sensors/package.xml Normal file
View File

@@ -0,0 +1,46 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
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.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_controller</name>
<version>0.0.1</version>
<description>
This Package is for Detection Math
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_external_sensors/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/PIDController.py'],
packages=['humanoid_robot_intelligence_control_system_controller'],
package_dir={'': 'src'},
)
setup(**setup_args)

50
humanoid_robot_intelligence_control_system_external_sensors/src/PIDController.py Normal file
View File

@@ -0,0 +1,50 @@
#!/usr/bin/env python3
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import time
from typing import Dict, List, Tuple
class PIDController:
def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))):
self.kp, self.ki, self.kd = gains
self.name = name
self.output_limits = output_limits
self.reset()
def reset(self):
self.last_error = 0
self.integral = 0
self.last_time = time.time()
def compute(self, setpoint: float, process_variable: float) -> float:
current_time = time.time()
dt = current_time - self.last_time
error = setpoint - process_variable
self.integral += error * dt
derivative = (error - self.last_error) / dt if dt > 0 else 0
output = self.kp * error + self.ki * self.integral + self.kd * derivative
output = max(min(output, self.output_limits[1]), self.output_limits[0])
self.last_error = error
self.last_time = current_time
return output
def update_config(self, gains: Tuple[float, float, float]):
self.kp, self.ki, self.kd = gains

60
humanoid_robot_intelligence_control_system_internal_sensors/CMakeLists.txt Normal file
View File

@@ -0,0 +1,60 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# 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.
cmake_minimum_required(VERSION 3.0.2)
project(humanoid_robot_intelligence_control_system_controller)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
rospy
)
catkin_package(
CATKIN_DEPENDS
rospy
)
else()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
endif()
if($ENV{ROS_VERSION} EQUAL 1)
catkin_python_setup()
catkin_install_python(PROGRAMS
src/PIDController.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY config launch rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
else()
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
src/PIDController.py
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY config launch rviz
DESTINATION share/${PROJECT_NAME}
)
ament_package()
endif()

46
humanoid_robot_intelligence_control_system_internal_sensors/package.xml Normal file
View File

@@ -0,0 +1,46 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
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.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_controller</name>
<version>0.0.1</version>
<description>
This Package is for Detection Math
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_internal_sensors/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/PIDController.py'],
packages=['humanoid_robot_intelligence_control_system_controller'],
package_dir={'': 'src'},
)
setup(**setup_args)

50
humanoid_robot_intelligence_control_system_internal_sensors/src/PIDController.py Normal file
View File

@@ -0,0 +1,50 @@
#!/usr/bin/env python3
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import time
from typing import Dict, List, Tuple
class PIDController:
def __init__(self, gains: Tuple[float, float, float], name: str, output_limits: Tuple[float, float] = (-float('inf'), float('inf'))):
self.kp, self.ki, self.kd = gains
self.name = name
self.output_limits = output_limits
self.reset()
def reset(self):
self.last_error = 0
self.integral = 0
self.last_time = time.time()
def compute(self, setpoint: float, process_variable: float) -> float:
current_time = time.time()
dt = current_time - self.last_time
error = setpoint - process_variable
self.integral += error * dt
derivative = (error - self.last_error) / dt if dt > 0 else 0
output = self.kp * error + self.ki * self.integral + self.kd * derivative
output = max(min(output, self.output_limits[1]), self.output_limits[0])
self.last_error = error
self.last_time = current_time
return output
def update_config(self, gains: Tuple[float, float, float]):
self.kp, self.ki, self.kd = gains

71
humanoid_robot_intelligence_control_system_object_detector/CMakeLists.txt Normal file
View File

@@ -0,0 +1,71 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# 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.
cmake_minimum_required(VERSION 3.0.2)
project(humanoid_robot_intelligence_control_system_object_detector)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
sensor_msgs
geometry_msgs
cv_bridge
)
catkin_package(
CATKIN_DEPENDS
rospy
std_msgs
sensor_msgs
geometry_msgs
cv_bridge
)
else()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
endif()
if($ENV{ROS_VERSION} EQUAL 1)
catkin_python_setup()
catkin_install_python(PROGRAMS
src/object_detection_processor.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY config launch rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
else()
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
src/object_detection_processor.py
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY config launch rviz
DESTINATION share/${PROJECT_NAME}
)
ament_package()
endif()

108
humanoid_robot_intelligence_control_system_object_detector/launch/object_detector_processor.launch.py Normal file
View File

@@ -0,0 +1,108 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import os
import sys
import subprocess
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def ros1_launch_description():
# Get command-line arguments
args = sys.argv[1:]
# Construct the ROS 1 launch command
roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_object_detector", "object_detector_processor.launch"] + args
roslaunch_command.extend([
"usb_cam", "usb_cam_node", "name:=camera",
"video_device:=/dev/video0",
"image_width:=640",
"image_height:=480",
"pixel_format:=yuyv",
"camera_frame_id:=usb_cam",
"io_method:=mmap"
])
roslaunch_command.extend([
"ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_object_detection.py", "name:=object_detection_node"
])
roslaunch_command.extend([
"humanoid_robot_intelligence_control_system_detector", "object_detection_processor.py", "name:=object_detection_processor_node"
])
roslaunch_command.extend([
"rviz", "rviz", "name:=rviz",
"args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz"
])
# Execute the launch command
subprocess.call(roslaunch_command)
def ros2_launch_description():
nodes_to_launch = []
nodes_to_launch.append(Node(
package='usb_cam',
executable='usb_cam_node',
name='camera',
output='screen',
parameters=[{
'video_device': '/dev/video0',
'image_width': 640,
'image_height': 480,
'pixel_format': 'yuyv',
'camera_frame_id': 'usb_cam',
'io_method': 'mmap'
}]
))
nodes_to_launch.append(Node(
package='ros_web_api_bellande_2d_computer_vision',
executable='bellande_2d_computer_vision_object_detection.py',
name='object_detection_node',
output='screen',
remappings=[('camera/image_raw', '/usb_cam/image_raw')]
))
nodes_to_launch.append(Node(
package='humanoid_robot_intelligence_control_system_object_detector',
executable='object_detection_processor.py',
name='object_detection_processor_node',
output='screen',
parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml'}]
))
nodes_to_launch.append(Node(
package='rviz2',
executable='rviz2',
name='rviz',
arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz']
))
return LaunchDescription(nodes_to_launch)
if __name__ == "__main__":
ros_version = os.getenv("ROS_VERSION")
if ros_version == "1":
ros1_launch_description()
elif ros_version == "2":
ros2_launch_description()
else:
print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.")
sys.exit(1)

41
humanoid_robot_intelligence_control_system_object_detector/launch/ros1/object_detector_processor.launch Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <https://www.gnu.org/licenses/>.
-->
<launch>
<!-- Launch USB camera -->
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
<!-- Launch object detection node -->
<node name="object_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_object_detection.py" output="screen">
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
</node>
<!-- Launch object detection processor node -->
<node name="object_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="object_detection_processor.py" output="screen">
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/object_detection_params.yaml"/>
</node>
<!-- Launch RViz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
</launch>

61
humanoid_robot_intelligence_control_system_object_detector/package.xml Normal file
View File

@@ -0,0 +1,61 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
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.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_object_detector</name>
<version>0.0.1</version>
<description>
This Package is for Object Detection, detecting objects like tools, or utilities
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<depend condition="$ROS_VERSION == 1">std_msgs</depend>
<depend condition="$ROS_VERSION == 1">sensor_msgs</depend>
<depend condition="$ROS_VERSION == 1">geometry_msgs</depend>
<depend condition="$ROS_VERSION == 1">cv_bridge</depend>
<depend condition="$ROS_VERSION == 1">image_transport</depend>
<depend condition="$ROS_VERSION == 1">dynamic_reconfigure</depend>
<depend condition="$ROS_VERSION == 1">message_generation</depend>
<depend condition="$ROS_VERSION == 1">message_runtime</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<depend condition="$ROS_VERSION == 2">std_msgs</depend>
<depend condition="$ROS_VERSION == 2">sensor_msgs</depend>
<depend condition="$ROS_VERSION == 2">geometry_msgs</depend>
<depend condition="$ROS_VERSION == 2">cv_bridge</depend>
<depend condition="$ROS_VERSION == 2">image_transport</depend>
<depend condition="$ROS_VERSION == 2">rosidl_default_generators</depend>
<depend condition="$ROS_VERSION == 2">rosidl_default_runtime</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_object_detector/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/object_detection_processor.py'],
packages=['humanoid_robot_intelligence_control_system_object_detector'],
package_dir={'': 'src'},
)
setup(**setup_args)

111
humanoid_robot_intelligence_control_system_object_detector/src/object_detection_processor.py Normal file
View File

@@ -0,0 +1,111 @@
#!/usr/bin/env python3
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CameraInfo
from std_msgs.msg import Bool, String
from cv_bridge import CvBridge
import yaml
class ObjectDetectionProcessor:
def __init__(self):
rospy.init_node('object_detection_processor')
self.bridge = CvBridge()
self.enable = True
self.new_image_flag = False
self.load_params()
self.setup_ros()
def load_params(self):
param_path = rospy.get_param('~yaml_path', '')
if param_path:
with open(param_path, 'r') as file:
self.params = yaml.safe_load(file)
else:
self.set_default_params()
def set_default_params(self):
self.params = {
'debug': False,
'ellipse_size': 2,
# Add other default parameters as needed
}
def setup_ros(self):
self.image_pub = rospy.Publisher('image_out', Image, queue_size=10)
self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10)
rospy.Subscriber('enable', Bool, self.enable_callback)
rospy.Subscriber('image_in', Image, self.image_callback)
rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback)
rospy.Subscriber('object_detection_result', String, self.object_detection_callback)
def enable_callback(self, msg):
self.enable = msg.data
def image_callback(self, msg):
if not self.enable:
return
self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
self.new_image_flag = True
self.image_header = msg.header
def camera_info_callback(self, msg):
if not self.enable:
return
self.camera_info_msg = msg
def object_detection_callback(self, msg):
if not self.enable or not hasattr(self, 'cv_image'):
return
objects = eval(msg.data) # Assuming the data is a string representation of a list
self.process_detected_objects(objects)
def process_detected_objects(self, objects):
output_image = self.cv_image.copy()
for obj in objects:
x, y, w, h = obj['bbox']
cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2)
cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}",
(int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2)
self.publish_image(output_image)
def publish_image(self, image):
img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
img_msg.header = self.image_header
self.image_pub.publish(img_msg)
if hasattr(self, 'camera_info_msg'):
self.camera_info_pub.publish(self.camera_info_msg)
def run(self):
rate = rospy.Rate(30) # 30 Hz
while not rospy.is_shutdown():
if self.new_image_flag:
# The processing is done in object_detection_callback
self.new_image_flag = False
rate.sleep()
if __name__ == '__main__':
try:
processor = ObjectDetectionProcessor()
processor.run()
except rospy.ROSInterruptException:
pass

71
humanoid_robot_intelligence_control_system_speech_detector/CMakeLists.txt Normal file
View File

@@ -0,0 +1,71 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# 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.
cmake_minimum_required(VERSION 3.0.2)
project(humanoid_robot_intelligence_control_system_speech_detector)
if($ENV{ROS_VERSION} EQUAL 1)
find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
sensor_msgs
geometry_msgs
cv_bridge
)
catkin_package(
CATKIN_DEPENDS
rospy
std_msgs
sensor_msgs
geometry_msgs
cv_bridge
)
else()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclpy REQUIRED)
find_package(std_msgs REQUIRED)
find_package(sensor_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(cv_bridge REQUIRED)
endif()
if($ENV{ROS_VERSION} EQUAL 1)
catkin_python_setup()
catkin_install_python(PROGRAMS
src/speech_detection_processor.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY config launch rviz
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
else()
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
src/speech_detection_processor.py
DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY config launch rviz
DESTINATION share/${PROJECT_NAME}
)
ament_package()
endif()

41
humanoid_robot_intelligence_control_system_speech_detector/launch/ros1/speech_detector_processor.launch Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <https://www.gnu.org/licenses/>.
-->
<launch>
<!-- Launch USB camera -->
<node name="usb_cam" pkg="usb_cam" type="usb_cam_node" output="screen">
<param name="video_device" value="/dev/video0" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_cam" />
<param name="io_method" value="mmap"/>
</node>
<!-- Launch speech detection node -->
<node name="speech_detection_node" pkg="ros_web_api_bellande_2d_computer_vision" type="bellande_2d_computer_vision_speech_detection.py" output="screen">
<remap from="camera/image_raw" to="/usb_cam/image_raw"/>
</node>
<!-- Launch speech detection processor node -->
<node name="speech_detection_processor_node" pkg="humanoid_robot_intelligence_control_system_ball_detector" type="speech_detection_processor.py" output="screen">
<param name="yaml_path" value="$(find ros_web_api_bellande_2d_computer_vision)/yaml/speech_detection_params.yaml"/>
</node>
<!-- Launch RViz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz" />
</launch>

108
humanoid_robot_intelligence_control_system_speech_detector/launch/speech_detector_processor.launch.py Normal file
View File

@@ -0,0 +1,108 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import os
import sys
import subprocess
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
def ros1_launch_description():
# Get command-line arguments
args = sys.argv[1:]
# Construct the ROS 1 launch command
roslaunch_command = ["roslaunch", "humanoid_robot_intelligence_control_system_speech_detector", "speech_detector_processor.launch"] + args
roslaunch_command.extend([
"usb_cam", "usb_cam_node", "name:=camera",
"video_device:=/dev/video0",
"image_width:=640",
"image_height:=480",
"pixel_format:=yuyv",
"camera_frame_id:=usb_cam",
"io_method:=mmap"
])
roslaunch_command.extend([
"ros_web_api_bellande_2d_computer_vision", "bellande_2d_computer_vision_speech_detection.py", "name:=speech_detection_node"
])
roslaunch_command.extend([
"humanoid_robot_intelligence_control_system_ball_detector", "speech_detection_processor.py", "name:=speech_detection_processor_node"
])
roslaunch_command.extend([
"rviz", "rviz", "name:=rviz",
"args:=-d $(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz"
])
# Execute the launch command
subprocess.call(roslaunch_command)
def ros2_launch_description():
nodes_to_launch = []
nodes_to_launch.append(Node(
package='usb_cam',
executable='usb_cam_node',
name='camera',
output='screen',
parameters=[{
'video_device': '/dev/video0',
'image_width': 640,
'image_height': 480,
'pixel_format': 'yuyv',
'camera_frame_id': 'usb_cam',
'io_method': 'mmap'
}]
))
nodes_to_launch.append(Node(
package='ros_web_api_bellande_2d_computer_vision',
executable='bellande_2d_computer_vision_speech_detection.py',
name='speech_detection_node',
output='screen',
remappings=[('camera/image_raw', '/usb_cam/image_raw')]
))
nodes_to_launch.append(Node(
package='humanoid_robot_intelligence_control_system_speech_detector',
executable='speech_detection_processor.py',
name='speech_detection_processor_node',
output='screen',
parameters=[{'yaml_path': '$(find ros_web_api_bellande_2d_computer_vision)/yaml/speech_detection_params.yaml'}]
))
nodes_to_launch.append(Node(
package='rviz2',
executable='rviz2',
name='rviz',
arguments=['-d', '$(find ros_web_api_bellande_2d_computer_vision)/rviz/visualization.rviz']
))
return LaunchDescription(nodes_to_launch)
if __name__ == "__main__":
ros_version = os.getenv("ROS_VERSION")
if ros_version == "1":
ros1_launch_description()
elif ros_version == "2":
ros2_launch_description()
else:
print("Unsupported ROS version. Please set the ROS_VERSION environment variable to '1' for ROS 1 or '2' for ROS 2.")
sys.exit(1)

61
humanoid_robot_intelligence_control_system_speech_detector/package.xml Normal file
View File

@@ -0,0 +1,61 @@
<?xml version="1.0"?>
<!--
Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
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.
-->
<package format="3">
<name>humanoid_robot_intelligence_control_system_speech_detector</name>
<version>0.0.1</version>
<description>
This Package is for Object Detection, detecting speechs like tools, or utilities
</description>
<license>Apache 2.0</license>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<buildtool_depend condition="$ROS_VERSION == 1">catkin</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake</buildtool_depend>
<buildtool_depend condition="$ROS_VERSION == 2">ament_cmake_python</buildtool_depend>
<!-- ROS 1 dependencies -->
<depend condition="$ROS_VERSION == 1">rospy</depend>
<depend condition="$ROS_VERSION == 1">std_msgs</depend>
<depend condition="$ROS_VERSION == 1">sensor_msgs</depend>
<depend condition="$ROS_VERSION == 1">geometry_msgs</depend>
<depend condition="$ROS_VERSION == 1">cv_bridge</depend>
<depend condition="$ROS_VERSION == 1">image_transport</depend>
<depend condition="$ROS_VERSION == 1">dynamic_reconfigure</depend>
<depend condition="$ROS_VERSION == 1">message_generation</depend>
<depend condition="$ROS_VERSION == 1">message_runtime</depend>
<!-- ROS 2 dependencies -->
<depend condition="$ROS_VERSION == 2">rclpy</depend>
<depend condition="$ROS_VERSION == 2">std_msgs</depend>
<depend condition="$ROS_VERSION == 2">sensor_msgs</depend>
<depend condition="$ROS_VERSION == 2">geometry_msgs</depend>
<depend condition="$ROS_VERSION == 2">cv_bridge</depend>
<depend condition="$ROS_VERSION == 2">image_transport</depend>
<depend condition="$ROS_VERSION == 2">rosidl_default_generators</depend>
<depend condition="$ROS_VERSION == 2">rosidl_default_runtime</depend>
<!-- Common dependencies -->
<depend>python3-opencv</depend>
<depend>python3-yaml</depend>
<depend>usb_cam</depend>
<export>
<build_type condition="$ROS_VERSION == 1">catkin</build_type>
<build_type condition="$ROS_VERSION == 2">ament_cmake</build_type>
</export>
</package>

26
humanoid_robot_intelligence_control_system_speech_detector/setup.py Normal file
View File

@@ -0,0 +1,26 @@
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
# fetch values from package.xml
setup_args = generate_distutils_setup(
scripts=['src/speech_detection_processor.py'],
packages=['humanoid_robot_intelligence_control_system_speech_detector'],
package_dir={'': 'src'},
)
setup(**setup_args)

111
humanoid_robot_intelligence_control_system_speech_detector/src/speech_detection_processor.py Normal file
View File

@@ -0,0 +1,111 @@
#!/usr/bin/env python3
# Copyright (C) 2024 Bellande Robotics Sensors Research Innovation Center, Ronaldson Bellande
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with this program. If not, see <https://www.gnu.org/licenses/>.
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CameraInfo
from std_msgs.msg import Bool, String
from cv_bridge import CvBridge
import yaml
class ObjectDetectionProcessor:
def __init__(self):
rospy.init_node('speech_detection_processor')
self.bridge = CvBridge()
self.enable = True
self.new_image_flag = False
self.load_params()
self.setup_ros()
def load_params(self):
param_path = rospy.get_param('~yaml_path', '')
if param_path:
with open(param_path, 'r') as file:
self.params = yaml.safe_load(file)
else:
self.set_default_params()
def set_default_params(self):
self.params = {
'debug': False,
'ellipse_size': 2,
# Add other default parameters as needed
}
def setup_ros(self):
self.image_pub = rospy.Publisher('image_out', Image, queue_size=10)
self.camera_info_pub = rospy.Publisher('camera_info', CameraInfo, queue_size=10)
rospy.Subscriber('enable', Bool, self.enable_callback)
rospy.Subscriber('image_in', Image, self.image_callback)
rospy.Subscriber('cameraInfo_in', CameraInfo, self.camera_info_callback)
rospy.Subscriber('speech_detection_result', String, self.speech_detection_callback)
def enable_callback(self, msg):
self.enable = msg.data
def image_callback(self, msg):
if not self.enable:
return
self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
self.new_image_flag = True
self.image_header = msg.header
def camera_info_callback(self, msg):
if not self.enable:
return
self.camera_info_msg = msg
def speech_detection_callback(self, msg):
if not self.enable or not hasattr(self, 'cv_image'):
return
speechs = eval(msg.data) # Assuming the data is a string representation of a list
self.process_detected_speechs(speechs)
def process_detected_speechs(self, speechs):
output_image = self.cv_image.copy()
for obj in speechs:
x, y, w, h = obj['bbox']
cv2.rectangle(output_image, (int(x), int(y)), (int(x+w), int(y+h)), (0, 255, 0), 2)
cv2.putText(output_image, f"{obj['label']}: {obj['confidence']:.2f}",
(int(x), int(y-10)), cv2.FONT_HERSHEY_SIMPLEX, 0.9, (0, 255, 0), 2)
self.publish_image(output_image)
def publish_image(self, image):
img_msg = self.bridge.cv2_to_imgmsg(image, encoding="bgr8")
img_msg.header = self.image_header
self.image_pub.publish(img_msg)
if hasattr(self, 'camera_info_msg'):
self.camera_info_pub.publish(self.camera_info_msg)
def run(self):
rate = rospy.Rate(30) # 30 Hz
while not rospy.is_shutdown():
if self.new_image_flag:
# The processing is done in speech_detection_callback
self.new_image_flag = False
rate.sleep()
if __name__ == '__main__':
try:
processor = ObjectDetectionProcessor()
processor.run()
except rospy.ROSInterruptException:
pass