Ronaldson Bellande
GitHub
commit
0c0679a668
30
.travis.yml
30
.travis.yml
@ -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
|
||||
|
||||
@ -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
157
humanoid_robot_intelligence_control_system_controller/CMakeLists.txt
Executable file → Normal 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()
|
||||
|
||||
@ -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
100
humanoid_robot_intelligence_control_system_controller/package.xml
Executable file → Normal 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>
|
||||
|
||||
@ -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)
|
||||
@ -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
|
||||
File diff suppressed because it is too large
Load Diff
@ -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()
|
||||
@ -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>
|
||||
@ -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)
|
||||
@ -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
|
||||
@ -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()
|
||||
@ -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>
|
||||
@ -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)
|
||||
@ -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
|
||||
@ -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()
|
||||
@ -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)
|
||||
@ -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>
|
||||
@ -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>
|
||||
@ -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)
|
||||
@ -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
|
||||
@ -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()
|
||||
@ -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>
|
||||
@ -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)
|
||||
@ -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>
|
||||
@ -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)
|
||||
@ -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
|
||||
Loading…
Reference in New Issue
Block a user