From 3dff932710cef856b448558a4fe2d968887a835b Mon Sep 17 00:00:00 2001 From: RonaldsonBellande Date: Fri, 6 May 2022 15:10:00 -0400 Subject: [PATCH] noetic build --- .../include/op3_ball_detector/ball_detector.h | 117 ++-- .../op3_ball_detector/ball_detector_config.h | 99 ++- op3_ball_detector/launch/ball_detector.launch | 9 +- .../launch/ball_detector_from_uvc.launch | 57 +- op3_ball_detector/package.xml | 62 +- op3_ball_detector/src/ball_detector.cpp | 651 ++++++++++-------- op3_ball_detector/src/ball_detector_node.cpp | 57 +- op3_bringup/CHANGELOG.rst | 5 + op3_bringup/CMakeLists.txt | 9 +- op3_bringup/launch/op3_bringup.launch | 8 +- .../launch/op3_bringup_visualization.launch | 8 +- op3_bringup/package.xml | 24 +- op3_demo/CHANGELOG.rst | 5 + op3_demo/CMakeLists.txt | 64 +- op3_demo/include/op3_demo/action_demo.h | 55 +- op3_demo/include/op3_demo/ball_follower.h | 87 ++- op3_demo/include/op3_demo/ball_tracker.h | 74 +- op3_demo/include/op3_demo/button_test.h | 46 +- op3_demo/include/op3_demo/face_tracker.h | 72 +- op3_demo/include/op3_demo/mic_test.h | 51 +- op3_demo/include/op3_demo/op_demo.h | 57 +- op3_demo/include/op3_demo/soccer_demo.h | 70 +- op3_demo/include/op3_demo/vision_demo.h | 54 +- op3_demo/launch/demo.launch | 45 +- op3_demo/launch/face_detection_op3.launch | 18 +- op3_demo/launch/self_test.launch | 17 +- op3_demo/list/action_script.yaml | 2 +- op3_demo/list/action_script_bk.yaml | 2 +- op3_demo/package.xml | 78 ++- op3_demo/src/action/action_demo.cpp | 345 ++++------ op3_demo/src/demo_node.cpp | 294 ++++---- op3_demo/src/soccer/ball_follower.cpp | 251 +++---- op3_demo/src/soccer/ball_tracker.cpp | 201 +++--- op3_demo/src/soccer/soccer_demo.cpp | 354 ++++------ op3_demo/src/test/button_test.cpp | 127 ++-- op3_demo/src/test/mic_test.cpp | 229 +++--- op3_demo/src/test_node.cpp | 365 +++++----- op3_demo/src/vision/face_tracker.cpp | 141 ++-- op3_demo/src/vision/vision_demo.cpp | 174 ++--- op3_read_write_demo/CHANGELOG.rst | 8 + op3_read_write_demo/CMakeLists.txt | 36 +- .../launch/op3_read_write.launch | 32 +- op3_read_write_demo/package.xml | 29 +- op3_read_write_demo/src/read_write.cpp | 171 ++--- robotis_op3_demo/CHANGELOG.rst | 5 + robotis_op3_demo/CMakeLists.txt | 2 +- robotis_op3_demo/package.xml | 20 +- 47 files changed, 2233 insertions(+), 2454 deletions(-) create mode 100644 op3_read_write_demo/CHANGELOG.rst diff --git a/op3_ball_detector/include/op3_ball_detector/ball_detector.h b/op3_ball_detector/include/op3_ball_detector/ball_detector.h index c429222..c6b4833 100644 --- a/op3_ball_detector/include/op3_ball_detector/ball_detector.h +++ b/op3_ball_detector/include/op3_ball_detector/ball_detector.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ @@ -21,88 +21,91 @@ #include -#include +#include +#include +#include #include -#include -#include +#include #include #include -#include -#include -#include +#include +#include +#include #include #include -#include #include #include "op3_ball_detector/ball_detector_config.h" -#include "op3_ball_detector/DetectorParamsConfig.h" #include "op3_ball_detector/CircleSetStamped.h" +#include "op3_ball_detector/DetectorParamsConfig.h" #include "op3_ball_detector/GetParameters.h" #include "op3_ball_detector/SetParameters.h" -namespace robotis_op -{ +namespace robotis_op { -class BallDetector -{ - public: +class BallDetector { +public: BallDetector(); ~BallDetector(); - //checks if a new image has been received + // checks if a new image has been received bool newImage(); - //execute circle detection with the cureent image + // execute circle detection with the cureent image void process(); - //publish the output image (input image + marked circles) + // publish the output image (input image + marked circles) void publishImage(); - //publish the circle set data + // publish the circle set data void publishCircles(); - protected: +protected: const static int NOT_FOUND_TH = 30; - //callbacks to image subscription - void imageCallback(const sensor_msgs::ImageConstPtr & msg); + // callbacks to image subscription + void imageCallback(const sensor_msgs::ImageConstPtr &msg); - //callbacks to camera info subscription - void cameraInfoCallback(const sensor_msgs::CameraInfo & msg); + // callbacks to camera info subscription + void cameraInfoCallback(const sensor_msgs::CameraInfo &msg); - void dynParamCallback(op3_ball_detector::DetectorParamsConfig &config, uint32_t level); + void dynParamCallback(op3_ball_detector::DetectorParamsConfig &config, + uint32_t level); void enableCallback(const std_msgs::Bool::ConstPtr &msg); void paramCommandCallback(const std_msgs::String::ConstPtr &msg); - bool setParamCallback(op3_ball_detector::SetParameters::Request &req, op3_ball_detector::SetParameters::Response &res); - bool getParamCallback(op3_ball_detector::GetParameters::Request &req, op3_ball_detector::GetParameters::Response &res); + bool setParamCallback(op3_ball_detector::SetParameters::Request &req, + op3_ball_detector::SetParameters::Response &res); + bool getParamCallback(op3_ball_detector::GetParameters::Request &req, + op3_ball_detector::GetParameters::Response &res); void resetParameter(); void publishParam(); void printConfig(); void saveConfig(); - void setInputImage(const cv::Mat & inIm); - void setInputImage(const cv::Mat & inIm, cv::Mat &in_filter_img); - void getOutputImage(cv::Mat & outIm); + void setInputImage(const cv::Mat &inIm); + void setInputImage(const cv::Mat &inIm, cv::Mat &in_filter_img); + void getOutputImage(cv::Mat &outIm); void filterImage(); void filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter_img); void makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range); void makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &mask_img); - void inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_value, cv::Mat &output_img); - void mophology(const cv::Mat &intput_img, cv::Mat &output_img, int ellipse_size); + void inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_value, + cv::Mat &output_img); + void mophology(const cv::Mat &intput_img, cv::Mat &output_img, + int ellipse_size); void houghDetection(const unsigned int imgEncoding); void houghDetection2(const cv::Mat &input_hough); void drawOutputImage(); - //ros node handle + // ros node handle ros::NodeHandle nh_; ros::Subscriber enable_sub_; - //image publisher/subscriber + // image publisher/subscriber image_transport::ImageTransport it_; image_transport::Publisher image_pub_; cv_bridge::CvImage cv_img_pub_; @@ -113,16 +116,16 @@ class BallDetector bool init_param_; int not_found_count_; - //circle set publisher + // circle set publisher op3_ball_detector::CircleSetStamped circles_msg_; ros::Publisher circles_pub_; - //camera info subscriber + // camera info subscriber sensor_msgs::CameraInfo camera_info_msg_; ros::Subscriber camera_info_sub_; ros::Publisher camera_info_pub_; - //dynamic reconfigure + // dynamic reconfigure DetectorConfig params_config_; std::string param_path_; bool has_path_; @@ -134,14 +137,14 @@ class BallDetector ros::ServiceServer get_param_client_; ros::ServiceServer set_param_client_; - //flag indicating a new image has been received + // flag indicating a new image has been received bool new_image_flag_; - //image time stamp and frame id + // image time stamp and frame id ros::Time sub_time_; std::string image_frame_id_; - //img encoding id + // img encoding id unsigned int img_encoding_; /** \brief Set of detected circles @@ -156,9 +159,11 @@ class BallDetector cv::Mat in_image_; cv::Mat out_image_; - dynamic_reconfigure::Server param_server_; - dynamic_reconfigure::Server::CallbackType callback_fnc_; + dynamic_reconfigure::Server + param_server_; + dynamic_reconfigure::Server< + op3_ball_detector::DetectorParamsConfig>::CallbackType callback_fnc_; }; -} // namespace robotis_op -#endif // _BALL_DETECTOR_H_ +} // namespace robotis_op +#endif // _BALL_DETECTOR_H_ diff --git a/op3_ball_detector/include/op3_ball_detector/ball_detector_config.h b/op3_ball_detector/include/op3_ball_detector/ball_detector_config.h index 6062145..a69be5d 100644 --- a/op3_ball_detector/include/op3_ball_detector/ball_detector_config.h +++ b/op3_ball_detector/include/op3_ball_detector/ball_detector_config.h @@ -1,28 +1,27 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef _DETECTOR_CONFIG_H_ #define _DETECTOR_CONFIG_H_ -namespace robotis_op -{ +namespace robotis_op { -//constants +// constants const int GAUSSIAN_BLUR_SIZE_DEFAULT = 7; const double GAUSSIAN_BLUR_SIGMA_DEFAULT = 2; const double CANNY_EDGE_TH_DEFAULT = 130; @@ -43,18 +42,12 @@ const int FILTER_V_MIN_DEFAULT = 0; const int FILTER_V_MAX_DEFAULT = 255; const int ELLIPSE_SIZE = 5; -class HsvFilter -{ - public: +class HsvFilter { +public: HsvFilter() - : h_min(FILTER_H_MIN_DEFAULT), - h_max(FILTER_H_MAX_DEFAULT), - s_min(FILTER_S_MIN_DEFAULT), - s_max(FILTER_S_MAX_DEFAULT), - v_min(FILTER_V_MIN_DEFAULT), - v_max(FILTER_V_MAX_DEFAULT) - { - } + : h_min(FILTER_H_MIN_DEFAULT), h_max(FILTER_H_MAX_DEFAULT), + s_min(FILTER_S_MIN_DEFAULT), s_max(FILTER_S_MAX_DEFAULT), + v_min(FILTER_V_MIN_DEFAULT), v_max(FILTER_V_MAX_DEFAULT) {} int h_min; int h_max; @@ -64,42 +57,34 @@ class HsvFilter int v_max; }; -class DetectorConfig -{ - public: - int gaussian_blur_size; // size of gaussian blur kernel mask [pixel] - double gaussian_blur_sigma; // sigma of gaussian blur kernel mask [pixel] - double canny_edge_th; // threshold of the edge detector. - double hough_accum_resolution; // resolution of the Hough accumulator, in terms of inverse ratio of image resolution - double min_circle_dist; // Minimum distance between circles - double hough_accum_th; // accumulator threshold to decide circle detection - int min_radius; // minimum circle radius allowed - int max_radius; // maximum circle radius allowed - HsvFilter filter_threshold; // filter threshold +class DetectorConfig { +public: + int gaussian_blur_size; // size of gaussian blur kernel mask [pixel] + double gaussian_blur_sigma; // sigma of gaussian blur kernel mask [pixel] + double canny_edge_th; // threshold of the edge detector. + double hough_accum_resolution; // resolution of the Hough accumulator, in + // terms of inverse ratio of image resolution + double min_circle_dist; // Minimum distance between circles + double hough_accum_th; // accumulator threshold to decide circle detection + int min_radius; // minimum circle radius allowed + int max_radius; // maximum circle radius allowed + HsvFilter filter_threshold; // filter threshold bool use_second_filter; - HsvFilter filter2_threshold; // filter threshold + HsvFilter filter2_threshold; // filter threshold int ellipse_size; - bool debug; // to debug log + bool debug; // to debug log DetectorConfig() : canny_edge_th(CANNY_EDGE_TH_DEFAULT), hough_accum_resolution(HOUGH_ACCUM_RESOLUTION_DEFAULT), min_circle_dist(MIN_CIRCLE_DIST_DEFAULT), - hough_accum_th(HOUGH_ACCUM_TH_DEFAULT), - min_radius(MIN_RADIUS_DEFAULT), - max_radius(MAX_RADIUS_DEFAULT), - filter_threshold(), - use_second_filter(false), - filter2_threshold(), - ellipse_size(ELLIPSE_SIZE), - debug(false) - { - } + hough_accum_th(HOUGH_ACCUM_TH_DEFAULT), min_radius(MIN_RADIUS_DEFAULT), + max_radius(MAX_RADIUS_DEFAULT), filter_threshold(), + use_second_filter(false), filter2_threshold(), + ellipse_size(ELLIPSE_SIZE), debug(false) {} - ~DetectorConfig() - { - } + ~DetectorConfig() {} }; -} -#endif // _DETECTOR_CONFIG_H_ +} // namespace robotis_op +#endif // _DETECTOR_CONFIG_H_ diff --git a/op3_ball_detector/launch/ball_detector.launch b/op3_ball_detector/launch/ball_detector.launch index 35f57d9..475eb62 100644 --- a/op3_ball_detector/launch/ball_detector.launch +++ b/op3_ball_detector/launch/ball_detector.launch @@ -1,13 +1,12 @@ - - + + - - + + - diff --git a/op3_ball_detector/launch/ball_detector_from_uvc.launch b/op3_ball_detector/launch/ball_detector_from_uvc.launch index 636caaa..f0ee90c 100644 --- a/op3_ball_detector/launch/ball_detector_from_uvc.launch +++ b/op3_ball_detector/launch/ball_detector_from_uvc.launch @@ -1,30 +1,29 @@ - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + - diff --git a/op3_ball_detector/package.xml b/op3_ball_detector/package.xml index d507794..3632328 100644 --- a/op3_ball_detector/package.xml +++ b/op3_ball_detector/package.xml @@ -10,26 +10,64 @@ Apache 2.0 Kayman Zerom - Pyo + Ronaldson Bellande + http://wiki.ros.org/op3_ball_detector http://emanual.robotis.com/docs/en/platform/op3/introduction/ https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - dynamic_reconfigure - cv_bridge - image_transport - boost - opencv3 - yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + dynamic_reconfigure + cv_bridge + image_transport + boost + opencv3 + yaml-cpp message_generation + message_runtime + message_runtime + usb_cam + uvc_camera + + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + dynamic_reconfigure + cv_bridge + image_transport + boost + opencv3 + yaml-cpp + message_generation message_runtime + message_runtime + usb_cam + uvc_camera + + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + dynamic_reconfigure + cv_bridge + image_transport + boost + opencv3 + yaml-cpp + message_generation + message_runtime message_runtime usb_cam uvc_camera + diff --git a/op3_ball_detector/src/ball_detector.cpp b/op3_ball_detector/src/ball_detector.cpp index 871a1e8..9659133 100644 --- a/op3_ball_detector/src/ball_detector.cpp +++ b/op3_ball_detector/src/ball_detector.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ @@ -20,66 +20,84 @@ #include "op3_ball_detector/ball_detector.h" -namespace robotis_op -{ +namespace robotis_op { BallDetector::BallDetector() - : nh_(ros::this_node::getName()), - it_(this->nh_), - enable_(true), - params_config_(), - init_param_(false), - not_found_count_(0) -{ + : nh_(ros::this_node::getName()), it_(this->nh_), enable_(true), + params_config_(), init_param_(false), not_found_count_(0) { has_path_ = nh_.getParam("yaml_path", param_path_); if (has_path_) std::cout << "Path : " << param_path_ << std::endl; - //detector config struct + // detector config struct DetectorConfig detect_config; - //get user parameters from dynamic reconfigure (yaml entries) - nh_.param("gaussian_blur_size", detect_config.gaussian_blur_size, params_config_.gaussian_blur_size); + // get user parameters from dynamic reconfigure (yaml entries) + nh_.param("gaussian_blur_size", detect_config.gaussian_blur_size, + params_config_.gaussian_blur_size); if (detect_config.gaussian_blur_size % 2 == 0) detect_config.gaussian_blur_size -= 1; if (detect_config.gaussian_blur_size <= 0) detect_config.gaussian_blur_size = 1; - nh_.param("gaussian_blur_sigma", detect_config.gaussian_blur_sigma, params_config_.gaussian_blur_sigma); - nh_.param("canny_edge_th", detect_config.canny_edge_th, params_config_.canny_edge_th); - nh_.param("hough_accum_resolution", detect_config.hough_accum_resolution, + nh_.param("gaussian_blur_sigma", detect_config.gaussian_blur_sigma, + params_config_.gaussian_blur_sigma); + nh_.param("canny_edge_th", detect_config.canny_edge_th, + params_config_.canny_edge_th); + nh_.param("hough_accum_resolution", + detect_config.hough_accum_resolution, params_config_.hough_accum_resolution); - nh_.param("min_circle_dist", detect_config.min_circle_dist, params_config_.min_circle_dist); - nh_.param("hough_accum_th", detect_config.hough_accum_th, params_config_.hough_accum_th); - nh_.param("min_radius", detect_config.min_radius, params_config_.min_radius); - nh_.param("max_radius", detect_config.max_radius, params_config_.max_radius); - nh_.param("filter_h_min", detect_config.filter_threshold.h_min, params_config_.filter_threshold.h_min); - nh_.param("filter_h_max", detect_config.filter_threshold.h_max, params_config_.filter_threshold.h_max); - nh_.param("filter_s_min", detect_config.filter_threshold.s_min, params_config_.filter_threshold.s_min); - nh_.param("filter_s_max", detect_config.filter_threshold.s_max, params_config_.filter_threshold.s_max); - nh_.param("filter_v_min", detect_config.filter_threshold.v_min, params_config_.filter_threshold.v_min); - nh_.param("filter_v_max", detect_config.filter_threshold.v_max, params_config_.filter_threshold.v_max); - nh_.param("use_second_filter", detect_config.use_second_filter, params_config_.use_second_filter); - nh_.param("filter2_h_min", detect_config.filter2_threshold.h_min, params_config_.filter2_threshold.h_min); - nh_.param("filter2_h_max", detect_config.filter2_threshold.h_max, params_config_.filter2_threshold.h_max); - nh_.param("filter2_s_min", detect_config.filter2_threshold.s_min, params_config_.filter2_threshold.s_min); - nh_.param("filter2_s_max", detect_config.filter2_threshold.s_max, params_config_.filter2_threshold.s_max); - nh_.param("filter2_v_min", detect_config.filter2_threshold.v_min, params_config_.filter2_threshold.v_min); - nh_.param("filter2_v_max", detect_config.filter2_threshold.v_max, params_config_.filter2_threshold.v_max); - nh_.param("ellipse_size", detect_config.ellipse_size, params_config_.ellipse_size); + nh_.param("min_circle_dist", detect_config.min_circle_dist, + params_config_.min_circle_dist); + nh_.param("hough_accum_th", detect_config.hough_accum_th, + params_config_.hough_accum_th); + nh_.param("min_radius", detect_config.min_radius, + params_config_.min_radius); + nh_.param("max_radius", detect_config.max_radius, + params_config_.max_radius); + nh_.param("filter_h_min", detect_config.filter_threshold.h_min, + params_config_.filter_threshold.h_min); + nh_.param("filter_h_max", detect_config.filter_threshold.h_max, + params_config_.filter_threshold.h_max); + nh_.param("filter_s_min", detect_config.filter_threshold.s_min, + params_config_.filter_threshold.s_min); + nh_.param("filter_s_max", detect_config.filter_threshold.s_max, + params_config_.filter_threshold.s_max); + nh_.param("filter_v_min", detect_config.filter_threshold.v_min, + params_config_.filter_threshold.v_min); + nh_.param("filter_v_max", detect_config.filter_threshold.v_max, + params_config_.filter_threshold.v_max); + nh_.param("use_second_filter", detect_config.use_second_filter, + params_config_.use_second_filter); + nh_.param("filter2_h_min", detect_config.filter2_threshold.h_min, + params_config_.filter2_threshold.h_min); + nh_.param("filter2_h_max", detect_config.filter2_threshold.h_max, + params_config_.filter2_threshold.h_max); + nh_.param("filter2_s_min", detect_config.filter2_threshold.s_min, + params_config_.filter2_threshold.s_min); + nh_.param("filter2_s_max", detect_config.filter2_threshold.s_max, + params_config_.filter2_threshold.s_max); + nh_.param("filter2_v_min", detect_config.filter2_threshold.v_min, + params_config_.filter2_threshold.v_min); + nh_.param("filter2_v_max", detect_config.filter2_threshold.v_max, + params_config_.filter2_threshold.v_max); + nh_.param("ellipse_size", detect_config.ellipse_size, + params_config_.ellipse_size); nh_.param("filter_debug", detect_config.debug, params_config_.debug); - //sets publishers + // sets publishers image_pub_ = it_.advertise("image_out", 100); - circles_pub_ = nh_.advertise("circle_set", 100); + circles_pub_ = + nh_.advertise("circle_set", 100); camera_info_pub_ = nh_.advertise("camera_info", 100); - //sets subscribers + // sets subscribers enable_sub_ = nh_.subscribe("enable", 1, &BallDetector::enableCallback, this); image_sub_ = it_.subscribe("image_in", 1, &BallDetector::imageCallback, this); - camera_info_sub_ = nh_.subscribe("cameraInfo_in", 100, &BallDetector::cameraInfoCallback, this); + camera_info_sub_ = nh_.subscribe("cameraInfo_in", 100, + &BallDetector::cameraInfoCallback, this); - //initializes newImageFlag + // initializes newImageFlag new_image_flag_ = false; // dynamic_reconfigure @@ -87,43 +105,39 @@ BallDetector::BallDetector() param_server_.setCallback(callback_fnc_); // web setting - param_pub_ = nh_.advertise("current_params", 1); - param_command_sub_ = nh_.subscribe("param_command", 1, &BallDetector::paramCommandCallback, this); - set_param_client_ = nh_.advertiseService("set_param", &BallDetector::setParamCallback, this); - get_param_client_ = nh_.advertiseService("get_param", &BallDetector::getParamCallback, this); - default_setting_path_ = ros::package::getPath(ROS_PACKAGE_NAME) + "/config/ball_detector_params_default.yaml"; + param_pub_ = + nh_.advertise("current_params", 1); + param_command_sub_ = nh_.subscribe("param_command", 1, + &BallDetector::paramCommandCallback, this); + set_param_client_ = + nh_.advertiseService("set_param", &BallDetector::setParamCallback, this); + get_param_client_ = + nh_.advertiseService("get_param", &BallDetector::getParamCallback, this); + default_setting_path_ = ros::package::getPath(ROS_PACKAGE_NAME) + + "/config/ball_detector_params_default.yaml"; - //sets config and prints it + // sets config and prints it params_config_ = detect_config; init_param_ = true; printConfig(); } -BallDetector::~BallDetector() -{ +BallDetector::~BallDetector() {} -} - -bool BallDetector::newImage() -{ - if (new_image_flag_) - { +bool BallDetector::newImage() { + if (new_image_flag_) { new_image_flag_ = false; return true; - } - else - { + } else { return false; } } -void BallDetector::process() -{ +void BallDetector::process() { if (enable_ == false) return; - if (cv_img_ptr_sub_ != NULL) - { + if (cv_img_ptr_sub_ != NULL) { cv::Mat img_hsv, img_filtered; // set input image @@ -132,31 +146,29 @@ void BallDetector::process() // image filtering filterImage(img_hsv, img_filtered); - //detect circles + // detect circles houghDetection2(img_filtered); -// // set input image -// setInputImage(cv_img_ptr_sub_->image); + // // set input image + // setInputImage(cv_img_ptr_sub_->image); -// // image filtering -// filterImage(); + // // image filtering + // filterImage(); -// //detect circles -// houghDetection(this->img_encoding_); + // //detect circles + // houghDetection(this->img_encoding_); } } -void BallDetector::publishImage() -{ +void BallDetector::publishImage() { if (enable_ == false) return; - //image_raw topic + // image_raw topic cv_img_pub_.header.seq++; cv_img_pub_.header.stamp = sub_time_; cv_img_pub_.header.frame_id = image_frame_id_; - switch (img_encoding_) - { + switch (img_encoding_) { case IMG_RGB8: cv_img_pub_.encoding = sensor_msgs::image_encodings::RGB8; break; @@ -172,69 +184,66 @@ void BallDetector::publishImage() camera_info_pub_.publish(camera_info_msg_); } -void BallDetector::publishCircles() -{ +void BallDetector::publishCircles() { if (enable_ == false) return; if (circles_.size() == 0) return; - //clears and resize the message + // clears and resize the message circles_msg_.circles.clear(); circles_msg_.circles.resize(circles_.size()); - //fill header + // fill header circles_msg_.header.seq++; circles_msg_.header.stamp = sub_time_; - circles_msg_.header.frame_id = "detector"; //To do: get frame_id from input image + circles_msg_.header.frame_id = + "detector"; // To do: get frame_id from input image - //fill circle data - // top(-1), bottom(+1) - // left(-1), right(+1) - for (int idx = 0; idx < circles_.size(); idx++) - { - circles_msg_.circles[idx].x = circles_[idx][0] / out_image_.cols * 2 - 1; // x (-1 ~ 1) - circles_msg_.circles[idx].y = circles_[idx][1] / out_image_.rows * 2 - 1; // y (-1 ~ 1) - circles_msg_.circles[idx].z = circles_[idx][2]; // radius + // fill circle data + // top(-1), bottom(+1) + // left(-1), right(+1) + for (int idx = 0; idx < circles_.size(); idx++) { + circles_msg_.circles[idx].x = + circles_[idx][0] / out_image_.cols * 2 - 1; // x (-1 ~ 1) + circles_msg_.circles[idx].y = + circles_[idx][1] / out_image_.rows * 2 - 1; // y (-1 ~ 1) + circles_msg_.circles[idx].z = circles_[idx][2]; // radius } - //publish message + // publish message circles_pub_.publish(circles_msg_); } -void BallDetector::enableCallback(const std_msgs::Bool::ConstPtr &msg) -{ +void BallDetector::enableCallback(const std_msgs::Bool::ConstPtr &msg) { enable_ = msg->data; } -void BallDetector::imageCallback(const sensor_msgs::ImageConstPtr & msg) -{ +void BallDetector::imageCallback(const sensor_msgs::ImageConstPtr &msg) { if (enable_ == false) return; - try - { + try { if (msg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0) this->img_encoding_ = IMG_MONO; if (msg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) this->img_encoding_ = IMG_RGB8; this->cv_img_ptr_sub_ = cv_bridge::toCvCopy(msg, msg->encoding); - } catch (cv_bridge::Exception& e) - { + } catch (cv_bridge::Exception &e) { ROS_ERROR("cv_bridge exception: %s", e.what()); return; } - //indicates a new image is available + // indicates a new image is available this->sub_time_ = msg->header.stamp; this->image_frame_id_ = msg->header.frame_id; this->new_image_flag_ = true; return; } -void BallDetector::dynParamCallback(op3_ball_detector::DetectorParamsConfig &config, uint32_t level) -{ +void BallDetector::dynParamCallback( + op3_ball_detector::DetectorParamsConfig &config, uint32_t level) { params_config_.gaussian_blur_size = config.gaussian_blur_size; params_config_.gaussian_blur_sigma = config.gaussian_blur_sigma; params_config_.canny_edge_th = config.canny_edge_th; @@ -269,35 +278,29 @@ void BallDetector::dynParamCallback(op3_ball_detector::DetectorParamsConfig &con saveConfig(); } -void BallDetector::cameraInfoCallback(const sensor_msgs::CameraInfo & msg) -{ +void BallDetector::cameraInfoCallback(const sensor_msgs::CameraInfo &msg) { if (enable_ == false) return; camera_info_msg_ = msg; } -void BallDetector::paramCommandCallback(const std_msgs::String::ConstPtr &msg) -{ - if(msg->data == "debug") - { +void BallDetector::paramCommandCallback(const std_msgs::String::ConstPtr &msg) { + if (msg->data == "debug") { params_config_.debug = true; saveConfig(); - } - else if(msg->data == "normal") - { + } else if (msg->data == "normal") { params_config_.debug = false; saveConfig(); - } - else if(msg->data == "reset") - { + } else if (msg->data == "reset") { // load default parameters and apply resetParameter(); } } -bool BallDetector::setParamCallback(op3_ball_detector::SetParameters::Request &req, op3_ball_detector::SetParameters::Response &res) -{ +bool BallDetector::setParamCallback( + op3_ball_detector::SetParameters::Request &req, + op3_ball_detector::SetParameters::Response &res) { params_config_.gaussian_blur_size = req.params.gaussian_blur_size; params_config_.gaussian_blur_sigma = req.params.gaussian_blur_sigma; params_config_.canny_edge_th = req.params.canny_edge_th; @@ -321,8 +324,9 @@ bool BallDetector::setParamCallback(op3_ball_detector::SetParameters::Request &r return true; } -bool BallDetector:: getParamCallback(op3_ball_detector::GetParameters::Request &req, op3_ball_detector::GetParameters::Response &res) -{ +bool BallDetector::getParamCallback( + op3_ball_detector::GetParameters::Request &req, + op3_ball_detector::GetParameters::Response &res) { res.returns.gaussian_blur_size = params_config_.gaussian_blur_size; res.returns.gaussian_blur_sigma = params_config_.gaussian_blur_sigma; res.returns.canny_edge_th = params_config_.canny_edge_th; @@ -342,21 +346,21 @@ bool BallDetector:: getParamCallback(op3_ball_detector::GetParameters::Request & return true; } -void BallDetector::resetParameter() -{ +void BallDetector::resetParameter() { YAML::Node doc; - try - { + try { // load yaml doc = YAML::LoadFile(default_setting_path_.c_str()); // parse params_config_.gaussian_blur_size = doc["gaussian_blur_size"].as(); - params_config_.gaussian_blur_sigma = doc["gaussian_blur_sigma"].as(); + params_config_.gaussian_blur_sigma = + doc["gaussian_blur_sigma"].as(); params_config_.canny_edge_th = doc["canny_edge_th"].as(); - params_config_.hough_accum_resolution = doc["hough_accum_resolution"].as(); + params_config_.hough_accum_resolution = + doc["hough_accum_resolution"].as(); params_config_.min_circle_dist = doc["min_circle_dist"].as(); params_config_.hough_accum_th = doc["hough_accum_th"].as(); params_config_.min_radius = doc["min_radius"].as(); @@ -387,15 +391,14 @@ void BallDetector::resetParameter() saveConfig(); publishParam(); - } catch (const std::exception& e) - { - ROS_ERROR_STREAM("Failed to Get default parameters : " << default_setting_path_); + } catch (const std::exception &e) { + ROS_ERROR_STREAM( + "Failed to Get default parameters : " << default_setting_path_); return; } } -void BallDetector::publishParam() -{ +void BallDetector::publishParam() { op3_ball_detector::BallDetectorParams params; params.gaussian_blur_size = params_config_.gaussian_blur_size; @@ -417,66 +420,110 @@ void BallDetector::publishParam() param_pub_.publish(params); } -void BallDetector::printConfig() -{ +void BallDetector::printConfig() { if (init_param_ == false) return; - std::cout << "Detetctor Configuration:" << std::endl << " gaussian_blur_size: " - << params_config_.gaussian_blur_size << std::endl << " gaussian_blur_sigma: " - << params_config_.gaussian_blur_sigma << std::endl << " canny_edge_th: " << params_config_.canny_edge_th - << std::endl << " hough_accum_resolution: " << params_config_.hough_accum_resolution << std::endl - << " min_circle_dist: " << params_config_.min_circle_dist << std::endl << " hough_accum_th: " - << params_config_.hough_accum_th << std::endl << " min_radius: " << params_config_.min_radius - << std::endl << " max_radius: " << params_config_.max_radius << std::endl << " filter_h_min: " - << params_config_.filter_threshold.h_min << std::endl << " filter_h_max: " - << params_config_.filter_threshold.h_max << std::endl << " filter_s_min: " - << params_config_.filter_threshold.s_min << std::endl << " filter_s_max: " - << params_config_.filter_threshold.s_max << std::endl << " filter_v_min: " - << params_config_.filter_threshold.v_min << std::endl << " filter_v_max: " - << params_config_.filter_threshold.v_max << std::endl << " use_second_filter: " - << params_config_.use_second_filter << std::endl << " filter2_h_min: " - << params_config_.filter2_threshold.h_min << std::endl << " filter2_h_max: " - << params_config_.filter2_threshold.h_max << std::endl << " filter2_s_min: " - << params_config_.filter2_threshold.s_min << std::endl << " filter2_s_max: " - << params_config_.filter2_threshold.s_max << std::endl << " filter2_v_min: " - << params_config_.filter2_threshold.v_min << std::endl << " filter2_v_max: " - << params_config_.filter2_threshold.v_max << std::endl << " ellipse_size: " - << params_config_.ellipse_size << std::endl << " filter_image_to_debug: " << params_config_.debug - << std::endl << std::endl; + std::cout << "Detetctor Configuration:" << std::endl + << " gaussian_blur_size: " << params_config_.gaussian_blur_size + << std::endl + << " gaussian_blur_sigma: " << params_config_.gaussian_blur_sigma + << std::endl + << " canny_edge_th: " << params_config_.canny_edge_th + << std::endl + << " hough_accum_resolution: " + << params_config_.hough_accum_resolution << std::endl + << " min_circle_dist: " << params_config_.min_circle_dist + << std::endl + << " hough_accum_th: " << params_config_.hough_accum_th + << std::endl + << " min_radius: " << params_config_.min_radius << std::endl + << " max_radius: " << params_config_.max_radius << std::endl + << " filter_h_min: " << params_config_.filter_threshold.h_min + << std::endl + << " filter_h_max: " << params_config_.filter_threshold.h_max + << std::endl + << " filter_s_min: " << params_config_.filter_threshold.s_min + << std::endl + << " filter_s_max: " << params_config_.filter_threshold.s_max + << std::endl + << " filter_v_min: " << params_config_.filter_threshold.v_min + << std::endl + << " filter_v_max: " << params_config_.filter_threshold.v_max + << std::endl + << " use_second_filter: " << params_config_.use_second_filter + << std::endl + << " filter2_h_min: " << params_config_.filter2_threshold.h_min + << std::endl + << " filter2_h_max: " << params_config_.filter2_threshold.h_max + << std::endl + << " filter2_s_min: " << params_config_.filter2_threshold.s_min + << std::endl + << " filter2_s_max: " << params_config_.filter2_threshold.s_max + << std::endl + << " filter2_v_min: " << params_config_.filter2_threshold.v_min + << std::endl + << " filter2_v_max: " << params_config_.filter2_threshold.v_max + << std::endl + << " ellipse_size: " << params_config_.ellipse_size << std::endl + << " filter_image_to_debug: " << params_config_.debug + << std::endl + << std::endl; } -void BallDetector::saveConfig() -{ +void BallDetector::saveConfig() { if (has_path_ == false) return; YAML::Emitter yaml_out; yaml_out << YAML::BeginMap; - yaml_out << YAML::Key << "gaussian_blur_size" << YAML::Value << params_config_.gaussian_blur_size; - yaml_out << YAML::Key << "gaussian_blur_sigma" << YAML::Value << params_config_.gaussian_blur_sigma; - yaml_out << YAML::Key << "canny_edge_th" << YAML::Value << params_config_.canny_edge_th; - yaml_out << YAML::Key << "hough_accum_resolution" << YAML::Value << params_config_.hough_accum_resolution; - yaml_out << YAML::Key << "min_circle_dist" << YAML::Value << params_config_.min_circle_dist; - yaml_out << YAML::Key << "hough_accum_th" << YAML::Value << params_config_.hough_accum_th; - yaml_out << YAML::Key << "min_radius" << YAML::Value << params_config_.min_radius; - yaml_out << YAML::Key << "max_radius" << YAML::Value << params_config_.max_radius; - yaml_out << YAML::Key << "filter_h_min" << YAML::Value << params_config_.filter_threshold.h_min; - yaml_out << YAML::Key << "filter_h_max" << YAML::Value << params_config_.filter_threshold.h_max; - yaml_out << YAML::Key << "filter_s_min" << YAML::Value << params_config_.filter_threshold.s_min; - yaml_out << YAML::Key << "filter_s_max" << YAML::Value << params_config_.filter_threshold.s_max; - yaml_out << YAML::Key << "filter_v_min" << YAML::Value << params_config_.filter_threshold.v_min; - yaml_out << YAML::Key << "filter_v_max" << YAML::Value << params_config_.filter_threshold.v_max; - yaml_out << YAML::Key << "use_second_filter" << YAML::Value << params_config_.use_second_filter; - yaml_out << YAML::Key << "filter2_h_min" << YAML::Value << params_config_.filter2_threshold.h_min; - yaml_out << YAML::Key << "filter2_h_max" << YAML::Value << params_config_.filter2_threshold.h_max; - yaml_out << YAML::Key << "filter2_s_min" << YAML::Value << params_config_.filter2_threshold.s_min; - yaml_out << YAML::Key << "filter2_s_max" << YAML::Value << params_config_.filter2_threshold.s_max; - yaml_out << YAML::Key << "filter2_v_min" << YAML::Value << params_config_.filter2_threshold.v_min; - yaml_out << YAML::Key << "filter2_v_max" << YAML::Value << params_config_.filter2_threshold.v_max; - yaml_out << YAML::Key << "ellipse_size" << YAML::Value << params_config_.ellipse_size; - yaml_out << YAML::Key << "filter_debug" << YAML::Value << params_config_.debug; + yaml_out << YAML::Key << "gaussian_blur_size" << YAML::Value + << params_config_.gaussian_blur_size; + yaml_out << YAML::Key << "gaussian_blur_sigma" << YAML::Value + << params_config_.gaussian_blur_sigma; + yaml_out << YAML::Key << "canny_edge_th" << YAML::Value + << params_config_.canny_edge_th; + yaml_out << YAML::Key << "hough_accum_resolution" << YAML::Value + << params_config_.hough_accum_resolution; + yaml_out << YAML::Key << "min_circle_dist" << YAML::Value + << params_config_.min_circle_dist; + yaml_out << YAML::Key << "hough_accum_th" << YAML::Value + << params_config_.hough_accum_th; + yaml_out << YAML::Key << "min_radius" << YAML::Value + << params_config_.min_radius; + yaml_out << YAML::Key << "max_radius" << YAML::Value + << params_config_.max_radius; + yaml_out << YAML::Key << "filter_h_min" << YAML::Value + << params_config_.filter_threshold.h_min; + yaml_out << YAML::Key << "filter_h_max" << YAML::Value + << params_config_.filter_threshold.h_max; + yaml_out << YAML::Key << "filter_s_min" << YAML::Value + << params_config_.filter_threshold.s_min; + yaml_out << YAML::Key << "filter_s_max" << YAML::Value + << params_config_.filter_threshold.s_max; + yaml_out << YAML::Key << "filter_v_min" << YAML::Value + << params_config_.filter_threshold.v_min; + yaml_out << YAML::Key << "filter_v_max" << YAML::Value + << params_config_.filter_threshold.v_max; + yaml_out << YAML::Key << "use_second_filter" << YAML::Value + << params_config_.use_second_filter; + yaml_out << YAML::Key << "filter2_h_min" << YAML::Value + << params_config_.filter2_threshold.h_min; + yaml_out << YAML::Key << "filter2_h_max" << YAML::Value + << params_config_.filter2_threshold.h_max; + yaml_out << YAML::Key << "filter2_s_min" << YAML::Value + << params_config_.filter2_threshold.s_min; + yaml_out << YAML::Key << "filter2_s_max" << YAML::Value + << params_config_.filter2_threshold.s_max; + yaml_out << YAML::Key << "filter2_v_min" << YAML::Value + << params_config_.filter2_threshold.v_min; + yaml_out << YAML::Key << "filter2_v_max" << YAML::Value + << params_config_.filter2_threshold.v_max; + yaml_out << YAML::Key << "ellipse_size" << YAML::Value + << params_config_.ellipse_size; + yaml_out << YAML::Key << "filter_debug" << YAML::Value + << params_config_.debug; yaml_out << YAML::EndMap; // output to file @@ -484,30 +531,26 @@ void BallDetector::saveConfig() fout << yaml_out.c_str(); } -void BallDetector::setInputImage(const cv::Mat & inIm) -{ +void BallDetector::setInputImage(const cv::Mat &inIm) { in_image_ = inIm.clone(); if (params_config_.debug == false) out_image_ = in_image_.clone(); } -void BallDetector::setInputImage(const cv::Mat & inIm, cv::Mat &in_filter_img) -{ +void BallDetector::setInputImage(const cv::Mat &inIm, cv::Mat &in_filter_img) { cv::cvtColor(inIm, in_filter_img, cv::COLOR_RGB2HSV); if (params_config_.debug == false) out_image_ = inIm.clone(); } -void BallDetector::getOutputImage(cv::Mat & outIm) -{ +void BallDetector::getOutputImage(cv::Mat &outIm) { this->drawOutputImage(); outIm = out_image_.clone(); } -void BallDetector::filterImage() -{ +void BallDetector::filterImage() { if (!in_image_.data) return; @@ -519,8 +562,7 @@ void BallDetector::filterImage() // mophology : open and close mophology(img_filtered, img_filtered, params_config_.ellipse_size); - if (params_config_.use_second_filter == true) - { + if (params_config_.use_second_filter == true) { // mask cv::Mat img_mask; @@ -540,8 +582,8 @@ void BallDetector::filterImage() cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB); } -void BallDetector::filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter_img) -{ +void BallDetector::filterImage(const cv::Mat &in_filter_img, + cv::Mat &out_filter_img) { if (!in_filter_img.data) return; @@ -550,8 +592,7 @@ void BallDetector::filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter // mophology : open and close mophology(out_filter_img, out_filter_img, params_config_.ellipse_size); - if (params_config_.use_second_filter == true) - { + if (params_config_.use_second_filter == true) { // mask cv::Mat img_mask; @@ -568,16 +609,16 @@ void BallDetector::filterImage(const cv::Mat &in_filter_img, cv::Mat &out_filter mophology(out_filter_img, out_filter_img, params_config_.ellipse_size); -// cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB); + // cv::cvtColor(img_filtered, in_image_, cv::COLOR_GRAY2RGB); - //draws results to output Image + // draws results to output Image if (params_config_.debug == true) cv::cvtColor(out_filter_img, out_image_, cv::COLOR_GRAY2RGB); -// out_image_ = in_image_.clone(); + // out_image_ = in_image_.clone(); } -void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, int range) -{ +void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, + int range) { // source_img. mask_img = cv::Mat::zeros(source_img.rows, source_img.cols, CV_8UC1); @@ -588,34 +629,30 @@ void BallDetector::makeFilterMask(const cv::Mat &source_img, cv::Mat &mask_img, if (source_img.channels() != 1) return; - for (int i = 0; i < source_height; i++) - { - for (int j = 0; j < source_width; j++) - { - uint8_t pixel = source_img.at < uint8_t > (i, j); + for (int i = 0; i < source_height; i++) { + for (int j = 0; j < source_width; j++) { + uint8_t pixel = source_img.at(i, j); if (pixel == 0) continue; - for (int mask_i = i - range; mask_i <= i + range; mask_i++) - { + for (int mask_i = i - range; mask_i <= i + range; mask_i++) { if (mask_i < 0 || mask_i >= source_height) continue; - for (int mask_j = j - range; mask_j <= j + range; mask_j++) - { + for (int mask_j = j - range; mask_j <= j + range; mask_j++) { if (mask_j < 0 || mask_j >= source_width) continue; - mask_img.at < uchar > (mask_i, mask_j, 0) = 255; + mask_img.at(mask_i, mask_j, 0) = 255; } } } } } -void BallDetector::makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &mask_img) -{ +void BallDetector::makeFilterMaskFromBall(const cv::Mat &source_img, + cv::Mat &mask_img) { // source_img. mask_img = cv::Mat::zeros(source_img.rows, source_img.cols, CV_8UC1); @@ -627,9 +664,9 @@ void BallDetector::makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &ma return; cv::Mat img_labels, stats, centroids; - int numOfLables = cv::connectedComponentsWithStats(source_img, img_labels, stats, centroids, 8, CV_32S); - for (int j = 1; j < numOfLables; j++) - { + int numOfLables = cv::connectedComponentsWithStats( + source_img, img_labels, stats, centroids, 8, CV_32S); + for (int j = 1; j < numOfLables; j++) { int area = stats.at(j, cv::CC_STAT_AREA); int left = stats.at(j, cv::CC_STAT_LEFT); int top = stats.at(j, cv::CC_STAT_TOP); @@ -640,48 +677,49 @@ void BallDetector::makeFilterMaskFromBall(const cv::Mat &source_img, cv::Mat &ma int center_y = top + height * 0.5; int radius = (width + height) * 0.5; - for (int mask_i = center_y - radius; mask_i <= center_y + radius; mask_i++) - { + for (int mask_i = center_y - radius; mask_i <= center_y + radius; + mask_i++) { if (mask_i < 0 || mask_i >= source_img.rows) continue; int mask_offset = abs(mask_i - center_y) * 0.5; - for (int mask_j = center_x - radius + mask_offset; mask_j <= center_x + radius - mask_offset; mask_j++) - { + for (int mask_j = center_x - radius + mask_offset; + mask_j <= center_x + radius - mask_offset; mask_j++) { if (mask_j < 0 || mask_j >= source_img.cols) continue; - mask_img.at < uchar > (mask_i, mask_j, 0) = 255; + mask_img.at(mask_i, mask_j, 0) = 255; } } } - } -void BallDetector::inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_value, cv::Mat &output_img) -{ +void BallDetector::inRangeHsv(const cv::Mat &input_img, + const HsvFilter &filter_value, + cv::Mat &output_img) { // 0-360 -> 0-180 int scaled_hue_min = static_cast(filter_value.h_min * 0.5); int scaled_hue_max = static_cast(filter_value.h_max * 0.5); - if (scaled_hue_min <= scaled_hue_max) - { - cv::Scalar min_value = cv::Scalar(scaled_hue_min, filter_value.s_min, filter_value.v_min, 0); - cv::Scalar max_value = cv::Scalar(scaled_hue_max, filter_value.s_max, filter_value.v_max, 0); + if (scaled_hue_min <= scaled_hue_max) { + cv::Scalar min_value = + cv::Scalar(scaled_hue_min, filter_value.s_min, filter_value.v_min, 0); + cv::Scalar max_value = + cv::Scalar(scaled_hue_max, filter_value.s_max, filter_value.v_max, 0); cv::inRange(input_img, min_value, max_value, output_img); - } - else - { + } else { cv::Mat lower_hue_range, upper_hue_range; cv::Scalar min_value, max_value; min_value = cv::Scalar(0, filter_value.s_min, filter_value.v_min, 0); - max_value = cv::Scalar(scaled_hue_max, filter_value.s_max, filter_value.v_max, 0); + max_value = + cv::Scalar(scaled_hue_max, filter_value.s_max, filter_value.v_max, 0); cv::inRange(input_img, min_value, max_value, lower_hue_range); - min_value = cv::Scalar(scaled_hue_min, filter_value.s_min, filter_value.v_min, 0); + min_value = + cv::Scalar(scaled_hue_min, filter_value.s_min, filter_value.v_min, 0); max_value = cv::Scalar(179, filter_value.s_max, filter_value.v_max, 0); cv::inRange(input_img, min_value, max_value, upper_hue_range); @@ -689,42 +727,50 @@ void BallDetector::inRangeHsv(const cv::Mat &input_img, const HsvFilter &filter_ } } -void BallDetector::mophology(const cv::Mat &intput_img, cv::Mat &output_img, int ellipse_size) -{ - cv::erode(intput_img, output_img, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size, ellipse_size))); - cv::dilate(output_img, output_img, - cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size * 2, ellipse_size * 2))); +void BallDetector::mophology(const cv::Mat &intput_img, cv::Mat &output_img, + int ellipse_size) { + cv::erode(intput_img, output_img, + cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Size(ellipse_size, ellipse_size))); + cv::dilate( + output_img, output_img, + cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Size(ellipse_size * 2, ellipse_size * 2))); cv::dilate(output_img, output_img, - cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size, ellipse_size))); - cv::erode(output_img, output_img, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(ellipse_size, ellipse_size))); + cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Size(ellipse_size, ellipse_size))); + cv::erode(output_img, output_img, + cv::getStructuringElement(cv::MORPH_ELLIPSE, + cv::Size(ellipse_size, ellipse_size))); } -void BallDetector::houghDetection(const unsigned int imgEncoding) -{ +void BallDetector::houghDetection(const unsigned int imgEncoding) { cv::Mat gray_image; std::vector circles_current; std::vector prev_circles = circles_; - //clear previous circles + // clear previous circles circles_.clear(); // If input image is RGB, convert it to gray if (imgEncoding == IMG_RGB8) cv::cvtColor(in_image_, gray_image, CV_RGB2GRAY); - //Reduce the noise so we avoid false circle detection + // Reduce the noise so we avoid false circle detection cv::GaussianBlur(gray_image, gray_image, - cv::Size(params_config_.gaussian_blur_size, params_config_.gaussian_blur_size), + cv::Size(params_config_.gaussian_blur_size, + params_config_.gaussian_blur_size), params_config_.gaussian_blur_sigma); double hough_accum_th = params_config_.hough_accum_th; - - //Apply the Hough Transform to find the circles - cv::HoughCircles(gray_image, circles_current, CV_HOUGH_GRADIENT, params_config_.hough_accum_resolution, - params_config_.min_circle_dist, params_config_.canny_edge_th, hough_accum_th, - params_config_.min_radius, params_config_.max_radius); + // Apply the Hough Transform to find the circles + cv::HoughCircles(gray_image, circles_current, CV_HOUGH_GRADIENT, + params_config_.hough_accum_resolution, + params_config_.min_circle_dist, params_config_.canny_edge_th, + hough_accum_th, params_config_.min_radius, + params_config_.max_radius); if (circles_current.size() == 0) not_found_count_ += 1; @@ -733,24 +779,23 @@ void BallDetector::houghDetection(const unsigned int imgEncoding) double alpha = 0.2; - for (int ix = 0; ix < circles_current.size(); ix++) - { - cv::Point2d center = cv::Point(circles_current[ix][0], circles_current[ix][1]); + for (int ix = 0; ix < circles_current.size(); ix++) { + cv::Point2d center = + cv::Point(circles_current[ix][0], circles_current[ix][1]); double radius = circles_current[ix][2]; - for (int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++) - { - cv::Point2d prev_center = cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]); + for (int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++) { + cv::Point2d prev_center = + cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]); double prev_radius = prev_circles[prev_ix][2]; cv::Point2d diff = center - prev_center; double radius_th = std::max(radius, prev_radius) * 0.75; - if (sqrt(diff.dot(diff)) < radius_th) - { - if (abs(radius - prev_radius) < radius_th) - { - circles_current[ix] = circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha); + if (sqrt(diff.dot(diff)) < radius_th) { + if (abs(radius - prev_radius) < radius_th) { + circles_current[ix] = + circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha); } break; @@ -761,32 +806,32 @@ void BallDetector::houghDetection(const unsigned int imgEncoding) } } -void BallDetector::houghDetection2(const cv::Mat &input_hough) -{ -// cv::Mat gray_image; +void BallDetector::houghDetection2(const cv::Mat &input_hough) { + // cv::Mat gray_image; std::vector circles_current; std::vector prev_circles = circles_; - //clear previous circles + // clear previous circles circles_.clear(); // If input image is RGB, convert it to gray -// if (imgEncoding == IMG_RGB8) -// cv::cvtColor(input_hough, gray_image, CV_RGB2GRAY); + // if (imgEncoding == IMG_RGB8) + // cv::cvtColor(input_hough, gray_image, CV_RGB2GRAY); - - //Reduce the noise so we avoid false circle detection + // Reduce the noise so we avoid false circle detection cv::GaussianBlur(input_hough, input_hough, - cv::Size(params_config_.gaussian_blur_size, params_config_.gaussian_blur_size), + cv::Size(params_config_.gaussian_blur_size, + params_config_.gaussian_blur_size), params_config_.gaussian_blur_sigma); double hough_accum_th = params_config_.hough_accum_th; - - //Apply the Hough Transform to find the circles - cv::HoughCircles(input_hough, circles_current, CV_HOUGH_GRADIENT, params_config_.hough_accum_resolution, - params_config_.min_circle_dist, params_config_.canny_edge_th, hough_accum_th, - params_config_.min_radius, params_config_.max_radius); + // Apply the Hough Transform to find the circles + cv::HoughCircles(input_hough, circles_current, CV_HOUGH_GRADIENT, + params_config_.hough_accum_resolution, + params_config_.min_circle_dist, params_config_.canny_edge_th, + hough_accum_th, params_config_.min_radius, + params_config_.max_radius); if (circles_current.size() == 0) not_found_count_ += 1; @@ -795,24 +840,23 @@ void BallDetector::houghDetection2(const cv::Mat &input_hough) double alpha = 0.2; - for (int ix = 0; ix < circles_current.size(); ix++) - { - cv::Point2d center = cv::Point(circles_current[ix][0], circles_current[ix][1]); + for (int ix = 0; ix < circles_current.size(); ix++) { + cv::Point2d center = + cv::Point(circles_current[ix][0], circles_current[ix][1]); double radius = circles_current[ix][2]; - for (int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++) - { - cv::Point2d prev_center = cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]); + for (int prev_ix = 0; prev_ix < prev_circles.size(); prev_ix++) { + cv::Point2d prev_center = + cv::Point(prev_circles[prev_ix][0], prev_circles[prev_ix][1]); double prev_radius = prev_circles[prev_ix][2]; cv::Point2d diff = center - prev_center; double radius_th = std::max(radius, prev_radius) * 0.75; - if (sqrt(diff.dot(diff)) < radius_th) - { - if (abs(radius - prev_radius) < radius_th) - { - circles_current[ix] = circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha); + if (sqrt(diff.dot(diff)) < radius_th) { + if (abs(radius - prev_radius) < radius_th) { + circles_current[ix] = + circles_current[ix] * alpha + prev_circles[prev_ix] * (1 - alpha); } break; @@ -823,30 +867,29 @@ void BallDetector::houghDetection2(const cv::Mat &input_hough) } } -void BallDetector::drawOutputImage() -{ +void BallDetector::drawOutputImage() { cv::Point center_position; int radius = 0; size_t ii; - //draws results to output Image -// if (params_config_.debug == true) -// out_image_ = in_image_.clone(); + // draws results to output Image + // if (params_config_.debug == true) + // out_image_ = in_image_.clone(); - for (ii = 0; ii < circles_.size(); ii++) - { + for (ii = 0; ii < circles_.size(); ii++) { { int this_radius = cvRound(circles_[ii][2]); - if (this_radius > radius) - { + if (this_radius > radius) { radius = this_radius; - center_position = cv::Point(cvRound(circles_[ii][0]), cvRound(circles_[ii][1])); + center_position = + cv::Point(cvRound(circles_[ii][0]), cvRound(circles_[ii][1])); } } } - cv::circle(out_image_, center_position, 5, cv::Scalar(0, 0, 255), -1, 8, 0); // circle center in blue - cv::circle(out_image_, center_position, radius, cv::Scalar(0, 0, 255), 3, 8, 0); // circle outline in blue - + cv::circle(out_image_, center_position, 5, cv::Scalar(0, 0, 255), -1, 8, + 0); // circle center in blue + cv::circle(out_image_, center_position, radius, cv::Scalar(0, 0, 255), 3, 8, + 0); // circle outline in blue } -} // namespace robotis_op +} // namespace robotis_op diff --git a/op3_ball_detector/src/ball_detector_node.cpp b/op3_ball_detector/src/ball_detector_node.cpp index 117127d..421ff6a 100644 --- a/op3_ball_detector/src/ball_detector_node.cpp +++ b/op3_ball_detector/src/ball_detector_node.cpp @@ -1,55 +1,50 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_ball_detector/ball_detector.h" -//node main -int main(int argc, char **argv) -{ - //init ros +// node main +int main(int argc, char **argv) { + // init ros ros::init(argc, argv, "ball_detector_node"); - //create ros wrapper object + // create ros wrapper object robotis_op::BallDetector detector; - //set node loop rate + // set node loop rate ros::Rate loop_rate(30); - //node loop - while (ros::ok()) - { - //if new image , do things - if (detector.newImage()) - { + // node loop + while (ros::ok()) { + // if new image , do things + if (detector.newImage()) { detector.process(); detector.publishImage(); detector.publishCircles(); - } - //execute pending callbacks + // execute pending callbacks ros::spinOnce(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } - //exit program + // exit program return 0; } - diff --git a/op3_bringup/CHANGELOG.rst b/op3_bringup/CHANGELOG.rst index bcad6b2..acc44a0 100644 --- a/op3_bringup/CHANGELOG.rst +++ b/op3_bringup/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package op3_bringup ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.0 (2021-05-05) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + 0.1.0 (2018-04-19) ------------------ * first release for ROS Kinetic diff --git a/op3_bringup/CMakeLists.txt b/op3_bringup/CMakeLists.txt index b171b00..629dcb4 100644 --- a/op3_bringup/CMakeLists.txt +++ b/op3_bringup/CMakeLists.txt @@ -1,7 +1,7 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(op3_bringup) ################################################################################ @@ -33,10 +33,11 @@ catkin_package() ################################################################################ # Install ################################################################################ -install(DIRECTORY launch rviz - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install( + DIRECTORY launch rviz + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) ################################################################################ # Test -################################################################################ \ No newline at end of file +################################################################################ diff --git a/op3_bringup/launch/op3_bringup.launch b/op3_bringup/launch/op3_bringup.launch index a3eacfb..63aa7c8 100644 --- a/op3_bringup/launch/op3_bringup.launch +++ b/op3_bringup/launch/op3_bringup.launch @@ -1,8 +1,8 @@ - - + + - - + + diff --git a/op3_bringup/launch/op3_bringup_visualization.launch b/op3_bringup/launch/op3_bringup_visualization.launch index 5ad06e0..0ce6a5f 100644 --- a/op3_bringup/launch/op3_bringup_visualization.launch +++ b/op3_bringup/launch/op3_bringup_visualization.launch @@ -1,10 +1,10 @@ - + - + - + [/robotis/present_joint_states] @@ -14,5 +14,5 @@ - + diff --git a/op3_bringup/package.xml b/op3_bringup/package.xml index 3161ae0..93ffc2f 100644 --- a/op3_bringup/package.xml +++ b/op3_bringup/package.xml @@ -1,23 +1,41 @@ op3_bringup - 0.1.0 + 0.3.0 This package is a demo for first time users. There is an example in the demo where you can run and visualize the robot. Apache 2.0 Kayman - Pyo + Ronaldson Bellande + http://wiki.ros.org/op3_bringup http://emanual.robotis.com/docs/en/platform/op3/introduction/ https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin + + op3_manager + op3_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + + op3_manager + op3_description + usb_cam + joint_state_publisher + robot_state_publisher + rviz + op3_manager op3_description usb_cam joint_state_publisher robot_state_publisher - rviz + rviz + diff --git a/op3_demo/CHANGELOG.rst b/op3_demo/CHANGELOG.rst index c28f305..9558d4d 100644 --- a/op3_demo/CHANGELOG.rst +++ b/op3_demo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package op3_demo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.0 (2021-05-05) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + 0.1.0 (2018-04-19) ------------------ * first release for ROS Kinetic diff --git a/op3_demo/CMakeLists.txt b/op3_demo/CMakeLists.txt index 381f872..c9baf11 100644 --- a/op3_demo/CMakeLists.txt +++ b/op3_demo/CMakeLists.txt @@ -1,13 +1,14 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(op3_demo) ################################################################################ # Find catkin packages and libraries for catkin and system dependencies ################################################################################ -find_package(catkin REQUIRED COMPONENTS +find_package( + catkin REQUIRED COMPONENTS roscpp roslib std_msgs @@ -29,18 +30,20 @@ find_package(Eigen3 REQUIRED) ## Insert your header file compatible specified path like '#include ' find_package(PkgConfig REQUIRED) pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) -find_path(YAML_CPP_INCLUDE_DIR +find_path( + YAML_CPP_INCLUDE_DIR NAMES yaml_cpp.h PATHS ${YAML_CPP_INCLUDE_DIRS} ) -find_library(YAML_CPP_LIBRARY +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) + add_definitions(-DHAVE_NEW_YAMLCPP) endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") ################################################################################ @@ -61,17 +64,17 @@ endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") catkin_package( INCLUDE_DIRS include CATKIN_DEPENDS - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - robotis_controller_msgs - op3_walking_module_msgs - op3_action_module_msgs - cmake_modules - robotis_math - op3_ball_detector + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + robotis_controller_msgs + op3_walking_module_msgs + op3_action_module_msgs + cmake_modules + robotis_math + op3_ball_detector DEPENDS Boost EIGEN3 ) @@ -86,7 +89,8 @@ include_directories( ${YAML_CPP_INCLUDE_DIRS} ) -add_executable(op_demo_node +add_executable( + op_demo_node src/demo_node.cpp src/soccer/soccer_demo.cpp src/soccer/ball_tracker.cpp @@ -96,19 +100,22 @@ add_executable(op_demo_node src/vision/face_tracker.cpp ) -add_dependencies(op_demo_node +add_dependencies( + op_demo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(op_demo_node +target_link_libraries( + op_demo_node ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} ${YAML_CPP_LIBRARIES} ) -add_executable(self_test_node +add_executable( + self_test_node src/test_node.cpp src/soccer/soccer_demo.cpp src/soccer/ball_tracker.cpp @@ -120,12 +127,14 @@ add_executable(self_test_node src/test/mic_test.cpp ) -add_dependencies(self_test_node +add_dependencies( + self_test_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(self_test_node +target_link_libraries( + self_test_node ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${Eigen3_LIBRARIES} @@ -135,16 +144,19 @@ target_link_libraries(self_test_node ################################################################################ # Install ################################################################################ -install(TARGETS op_demo_node self_test_node +install( + TARGETS op_demo_node self_test_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY include/${PROJECT_NAME}/ +install( + DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(DIRECTORY data launch list - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install( + DIRECTORY data launch list + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) ################################################################################ diff --git a/op3_demo/include/op3_demo/action_demo.h b/op3_demo/include/op3_demo/action_demo.h index 4231406..b3ff459 100644 --- a/op3_demo/include/op3_demo/action_demo.h +++ b/op3_demo/include/op3_demo/action_demo.h @@ -1,58 +1,54 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef ACTION_DEMO_H_ #define ACTION_DEMO_H_ -#include #include +#include #include #include #include #include +#include "op3_action_module_msgs/IsRunning.h" #include "op3_demo/op_demo.h" #include "robotis_controller_msgs/JointCtrlModule.h" #include "robotis_controller_msgs/SetModule.h" -#include "op3_action_module_msgs/IsRunning.h" -namespace robotis_op -{ +namespace robotis_op { -class ActionDemo : public OPDemo -{ - public: +class ActionDemo : public OPDemo { +public: ActionDemo(); ~ActionDemo(); void setDemoEnable(); void setDemoDisable(); - protected: - enum ActionCommandIndex - { +protected: + enum ActionCommandIndex { BrakeActionCommand = -2, StopActionCommand = -1, }; - enum ActionStatus - { + enum ActionStatus { PlayAction = 1, PauseAction = 2, StopAction = 3, @@ -74,7 +70,8 @@ class ActionDemo : public OPDemo void handleStatus(); void parseActionScript(const std::string &path); - bool parseActionScriptSetName(const std::string &path, const std::string &set_name); + bool parseActionScriptSetName(const std::string &path, + const std::string &set_name); bool playActionWithSound(int motion_index); @@ -88,8 +85,8 @@ class ActionDemo : public OPDemo void setModuleToDemo(const std::string &module_name); void callServiceSettingModule(const std::string &module_name); - void actionSetNameCallback(const std_msgs::String::ConstPtr& msg); - void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); + void actionSetNameCallback(const std_msgs::String::ConstPtr &msg); + void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); void demoCommandCallback(const std_msgs::String::ConstPtr &msg); ros::Publisher module_control_pub_; diff --git a/op3_demo/include/op3_demo/ball_follower.h b/op3_demo/include/op3_demo/ball_follower.h index 232c6e7..06984e7 100644 --- a/op3_demo/include/op3_demo/ball_follower.h +++ b/op3_demo/include/op3_demo/ball_follower.h @@ -1,48 +1,45 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef BALL_FOLLOWER_H_ #define BALL_FOLLOWER_H_ +#include #include #include -#include #include -#include -#include +#include #include -#include +#include +#include #include -#include "robotis_controller_msgs/JointCtrlModule.h" #include "op3_ball_detector/CircleSetStamped.h" -#include "op3_walking_module_msgs/WalkingParam.h" #include "op3_walking_module_msgs/GetWalkingParam.h" +#include "op3_walking_module_msgs/WalkingParam.h" +#include "robotis_controller_msgs/JointCtrlModule.h" -namespace robotis_op -{ +namespace robotis_op { // following the ball using walking -class BallFollower -{ - public: - enum - { +class BallFollower { +public: + enum { NotFound = 0, OutOfRange = 1, OnRight = 2, @@ -57,22 +54,16 @@ class BallFollower void waitFollowing(); void startFollowing(); void stopFollowing(); - void clearBallPosition() - { - approach_ball_position_ = NotFound; + void clearBallPosition() { approach_ball_position_ = NotFound; } + + int getBallPosition() { return approach_ball_position_; } + + bool isBallInRange() { + return (approach_ball_position_ == OnRight || + approach_ball_position_ == OnLeft); } - int getBallPosition() - { - return approach_ball_position_; - } - - bool isBallInRange() - { - return (approach_ball_position_ == OnRight || approach_ball_position_ == OnLeft); - } - - protected: +protected: const bool DEBUG_PRINT; const double CAMERA_HEIGHT; const int NOT_FOUND_THRESHOLD; @@ -92,15 +83,16 @@ class BallFollower void currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void setWalkingCommand(const std::string &command); - void setWalkingParam(double x_move, double y_move, double rotation_angle, bool balance = true); + void setWalkingParam(double x_move, double y_move, double rotation_angle, + bool balance = true); bool getWalkingParam(); - void calcFootstep(double target_distance, double target_angle, double delta_time, - double& fb_move, double& rl_angle); + void calcFootstep(double target_distance, double target_angle, + double delta_time, double &fb_move, double &rl_angle); - //ros node handle + // ros node handle ros::NodeHandle nh_; - //image publisher/subscriber + // image publisher/subscriber ros::Publisher module_control_pub_; ros::Publisher head_joint_pub_; ros::Publisher head_scan_pub_; @@ -131,8 +123,7 @@ class BallFollower double curr_period_time_; double accum_period_time_; - }; -} +} // namespace robotis_op #endif /* BALL_FOLLOWER_H_ */ diff --git a/op3_demo/include/op3_demo/ball_tracker.h b/op3_demo/include/op3_demo/ball_tracker.h index 383c809..f6d996e 100644 --- a/op3_demo/include/op3_demo/ball_tracker.h +++ b/op3_demo/include/op3_demo/ball_tracker.h @@ -1,47 +1,44 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef BALL_TRACKING_H_ #define BALL_TRACKING_H_ -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include #include -#include "robotis_controller_msgs/JointCtrlModule.h" #include "op3_ball_detector/CircleSetStamped.h" -#include "op3_walking_module_msgs/WalkingParam.h" #include "op3_walking_module_msgs/GetWalkingParam.h" +#include "op3_walking_module_msgs/WalkingParam.h" +#include "robotis_controller_msgs/JointCtrlModule.h" -namespace robotis_op -{ +namespace robotis_op { // head tracking for looking the ball -class BallTracker -{ +class BallTracker { public: - enum TrackingStatus - { + enum TrackingStatus { NotFound = -1, Waiting = 0, Found = 1, @@ -58,20 +55,15 @@ public: void setUsingHeadScan(bool use_scan); void goInit(); - double getPanOfBall() - { + double getPanOfBall() { // left (+) ~ right (-) return current_ball_pan_; } - double getTiltOfBall() - { + double getTiltOfBall() { // top (+) ~ bottom (-) return current_ball_tilt_; } - double getBallSize() - { - return current_ball_bottom_; - } + double getBallSize() { return current_ball_bottom_; } protected: const double FOV_WIDTH; @@ -80,15 +72,16 @@ protected: const int WAITING_THRESHOLD; const bool DEBUG_PRINT; - void ballPositionCallback(const op3_ball_detector::CircleSetStamped::ConstPtr &msg); + void ballPositionCallback( + const op3_ball_detector::CircleSetStamped::ConstPtr &msg); void ballTrackerCommandCallback(const std_msgs::String::ConstPtr &msg); void publishHeadJoint(double pan, double tilt); void scanBall(); - //ros node handle + // ros node handle ros::NodeHandle nh_; - //image publisher/subscriber + // image publisher/subscriber ros::Publisher module_control_pub_; ros::Publisher head_joint_offset_pub_; ros::Publisher head_joint_pub_; @@ -114,8 +107,7 @@ protected: double x_error_sum_, y_error_sum_; ros::Time prev_time_; double p_gain_, d_gain_, i_gain_; - }; -} +} // namespace robotis_op #endif /* BALL_TRACKING_H_ */ diff --git a/op3_demo/include/op3_demo/button_test.h b/op3_demo/include/op3_demo/button_test.h index 84e8bee..0090702 100644 --- a/op3_demo/include/op3_demo/button_test.h +++ b/op3_demo/include/op3_demo/button_test.h @@ -1,45 +1,43 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef BUTTON_TEST_H_ #define BUTTON_TEST_H_ -#include -#include -#include #include +#include +#include +#include #include "op3_demo/op_demo.h" #include "robotis_controller_msgs/SyncWriteItem.h" -namespace robotis_op -{ +namespace robotis_op { -class ButtonTest : public OPDemo -{ - public: +class ButtonTest : public OPDemo { +public: ButtonTest(); ~ButtonTest(); void setDemoEnable(); void setDemoDisable(); - protected: +protected: const int SPIN_RATE; void processThread(); @@ -52,7 +50,7 @@ class ButtonTest : public OPDemo void playSound(const std::string &path); - void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); + void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); ros::Publisher rgb_led_pub_; ros::Publisher play_sound_pub_; diff --git a/op3_demo/include/op3_demo/face_tracker.h b/op3_demo/include/op3_demo/face_tracker.h index 79b3d13..a1b7abc 100644 --- a/op3_demo/include/op3_demo/face_tracker.h +++ b/op3_demo/include/op3_demo/face_tracker.h @@ -1,43 +1,40 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef FACE_TRACKING_H_ #define FACE_TRACKING_H_ -#include -#include -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include #include -namespace robotis_op -{ +namespace robotis_op { // head tracking for looking the ball -class FaceTracker -{ - public: - enum TrackingStatus - { +class FaceTracker { +public: + enum TrackingStatus { NotFound = -1, Waiting = 0, Found = 1, @@ -56,16 +53,10 @@ class FaceTracker void setFacePosition(geometry_msgs::Point &face_position); void goInit(double init_pan, double init_tile); - double getPanOfFace() - { - return current_face_pan_; - } - double getTiltOfFace() - { - return current_face_tilt_; - } + double getPanOfFace() { return current_face_pan_; } + double getTiltOfFace() { return current_face_tilt_; } - protected: +protected: const double FOV_WIDTH; const double FOV_HEIGHT; const int NOT_FOUND_THRESHOLD; @@ -75,10 +66,10 @@ class FaceTracker void publishHeadJoint(double pan, double tilt); void scanFace(); - //ros node handle + // ros node handle ros::NodeHandle nh_; - //image publisher/subscriber + // image publisher/subscriber ros::Publisher module_control_pub_; ros::Publisher head_joint_offset_pub_; ros::Publisher head_joint_pub_; @@ -97,8 +88,7 @@ class FaceTracker double current_face_pan_, current_face_tilt_; int dismissed_count_; - }; -} +} // namespace robotis_op #endif /* FACE_TRACKING_H_ */ diff --git a/op3_demo/include/op3_demo/mic_test.h b/op3_demo/include/op3_demo/mic_test.h index 144fe96..ceb2f17 100644 --- a/op3_demo/include/op3_demo/mic_test.h +++ b/op3_demo/include/op3_demo/mic_test.h @@ -1,41 +1,38 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef MIC_TEST_H_ #define MIC_TEST_H_ -#include -#include -#include -#include #include +#include +#include +#include +#include #include "op3_demo/op_demo.h" #include "robotis_controller_msgs/SyncWriteItem.h" -namespace robotis_op -{ +namespace robotis_op { -class MicTest : public OPDemo -{ - public: - enum Mic_Test_Status - { +class MicTest : public OPDemo { +public: + enum Mic_Test_Status { Ready = 0, AnnounceRecording = 1, MicRecording = 2, @@ -50,7 +47,7 @@ class MicTest : public OPDemo void setDemoEnable(); void setDemoDisable(); - protected: +protected: const int SPIN_RATE; void processThread(); @@ -68,7 +65,7 @@ class MicTest : public OPDemo void startTimer(double wait_time); void finishTimer(); - void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); + void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); std::string recording_file_name_; std::string default_mp3_path_; diff --git a/op3_demo/include/op3_demo/op_demo.h b/op3_demo/include/op3_demo/op_demo.h index 25641a0..54cb72b 100644 --- a/op3_demo/include/op3_demo/op_demo.h +++ b/op3_demo/include/op3_demo/op_demo.h @@ -1,32 +1,29 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef OP_DEMO_H_ #define OP_DEMO_H_ -namespace robotis_op -{ +namespace robotis_op { -class OPDemo -{ - public: - enum Motion_Index - { +class OPDemo { +public: + enum Motion_Index { InitPose = 1, WalkingReady = 9, GetUpFront = 122, @@ -37,21 +34,13 @@ class OPDemo ForGrass = 20, }; - OPDemo() - { - } - virtual ~OPDemo() - { - } + OPDemo() {} + virtual ~OPDemo() {} - virtual void setDemoEnable() - { - } - virtual void setDemoDisable() - { - } + virtual void setDemoEnable() {} + virtual void setDemoDisable() {} - protected: +protected: bool enable_; }; diff --git a/op3_demo/include/op3_demo/soccer_demo.h b/op3_demo/include/op3_demo/soccer_demo.h index 48fd957..ae7b553 100644 --- a/op3_demo/include/op3_demo/soccer_demo.h +++ b/op3_demo/include/op3_demo/soccer_demo.h @@ -1,56 +1,52 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef SOCCER_DEMO_H #define SOCCER_DEMO_H -#include -#include -#include #include #include +#include +#include +#include #include #include "op3_action_module_msgs/IsRunning.h" -#include "robotis_controller_msgs/SyncWriteItem.h" #include "robotis_controller_msgs/JointCtrlModule.h" #include "robotis_controller_msgs/SetJointModule.h" +#include "robotis_controller_msgs/SyncWriteItem.h" -#include "op3_demo/op_demo.h" -#include "op3_demo/ball_tracker.h" #include "op3_demo/ball_follower.h" +#include "op3_demo/ball_tracker.h" +#include "op3_demo/op_demo.h" #include "robotis_math/robotis_linear_algebra.h" -namespace robotis_op -{ +namespace robotis_op { -class SoccerDemo : public OPDemo -{ - public: - enum Stand_Status - { +class SoccerDemo : public OPDemo { +public: + enum Stand_Status { Stand = 0, Fallen_Forward = 1, Fallen_Behind = 2, }; - enum Robot_Status - { + enum Robot_Status { Waited = 0, TrackingAndFollowing = 1, ReadyToKick = 2, @@ -64,7 +60,7 @@ class SoccerDemo : public OPDemo void setDemoEnable(); void setDemoDisable(); - protected: +protected: const double FALL_FORWARD_LIMIT; const double FALL_BACK_LIMIT; const int SPIN_RATE; @@ -74,17 +70,19 @@ class SoccerDemo : public OPDemo void callbackThread(); void trackingThread(); - void setBodyModuleToDemo(const std::string &body_module, bool with_head_control = true); + void setBodyModuleToDemo(const std::string &body_module, + bool with_head_control = true); void setModuleToDemo(const std::string &module_name); - void callServiceSettingModule(const robotis_controller_msgs::JointCtrlModule &modules); + void callServiceSettingModule( + const robotis_controller_msgs::JointCtrlModule &modules); void parseJointNameFromYaml(const std::string &path); bool getJointNameFromID(const int &id, std::string &joint_name); bool getIDFromJointName(const std::string &joint_name, int &id); int getJointCount(); bool isHeadJoint(const int &id); - void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); - void demoCommandCallback(const std_msgs::String::ConstPtr& msg); - void imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg); + void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); + void demoCommandCallback(const std_msgs::String::ConstPtr &msg); + void imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg); void startSoccerMode(); void stopSoccerMode(); @@ -132,5 +130,5 @@ class SoccerDemo : public OPDemo double present_pitch_; }; -} // namespace robotis_op +} // namespace robotis_op #endif // SOCCER_DEMO_H diff --git a/op3_demo/include/op3_demo/vision_demo.h b/op3_demo/include/op3_demo/vision_demo.h index d97bc41..523bcc8 100644 --- a/op3_demo/include/op3_demo/vision_demo.h +++ b/op3_demo/include/op3_demo/vision_demo.h @@ -1,49 +1,47 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #ifndef VISION_DEMO_H_ #define VISION_DEMO_H_ -#include -#include -#include -#include #include +#include +#include +#include +#include -#include "robotis_controller_msgs/SyncWriteItem.h" #include "robotis_controller_msgs/SetModule.h" +#include "robotis_controller_msgs/SyncWriteItem.h" -#include "op3_demo/op_demo.h" #include "op3_demo/face_tracker.h" +#include "op3_demo/op_demo.h" -namespace robotis_op -{ +namespace robotis_op { -class VisionDemo : public OPDemo -{ - public: +class VisionDemo : public OPDemo { +public: VisionDemo(); ~VisionDemo(); void setDemoEnable(); void setDemoDisable(); - protected: +protected: const int SPIN_RATE; const int TIME_TO_INIT; @@ -55,7 +53,7 @@ class VisionDemo : public OPDemo void playMotion(int motion_index); void setRGBLED(int blue, int green, int red); - void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); + void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); void facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg); void demoCommandCallback(const std_msgs::String::ConstPtr &msg); @@ -66,7 +64,7 @@ class VisionDemo : public OPDemo ros::Publisher module_control_pub_; ros::Publisher motion_index_pub_; - ros::Publisher rgb_led_pub_; + ros::Publisher rgb_led_pub_; ros::Publisher face_tracking_command_pub_; ros::Subscriber buttuon_sub_; diff --git a/op3_demo/launch/demo.launch b/op3_demo/launch/demo.launch index 0bd83a9..dc0b421 100644 --- a/op3_demo/launch/demo.launch +++ b/op3_demo/launch/demo.launch @@ -1,28 +1,27 @@ - - + + - - - - - + + - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - diff --git a/op3_demo/launch/face_detection_op3.launch b/op3_demo/launch/face_detection_op3.launch index d42ab41..e1ea7ef 100644 --- a/op3_demo/launch/face_detection_op3.launch +++ b/op3_demo/launch/face_detection_op3.launch @@ -1,22 +1,20 @@ - - - - - + + + + + - + $(arg face_cascade_name_4)" output="screen"> - + diff --git a/op3_demo/launch/self_test.launch b/op3_demo/launch/self_test.launch index 00c9acb..1979ba7 100644 --- a/op3_demo/launch/self_test.launch +++ b/op3_demo/launch/self_test.launch @@ -1,24 +1,24 @@ - + - + - - + + - + - - + + - + @@ -26,4 +26,3 @@ - diff --git a/op3_demo/list/action_script.yaml b/op3_demo/list/action_script.yaml index 21ce72b..18ae60f 100644 --- a/op3_demo/list/action_script.yaml +++ b/op3_demo/list/action_script.yaml @@ -20,4 +20,4 @@ prev_default: [4, 41, 24, 23, 15, 1, 54, 27, 38] default: [4, 110, 111, 115, 118, 24, 54, 27, 38] # example of play list -#certification: [101] \ No newline at end of file +#certification: [101] diff --git a/op3_demo/list/action_script_bk.yaml b/op3_demo/list/action_script_bk.yaml index bc401fb..2dd155b 100644 --- a/op3_demo/list/action_script_bk.yaml +++ b/op3_demo/list/action_script_bk.yaml @@ -15,4 +15,4 @@ action_and_sound: default: [4, 41, 24, 23, 15, 1, 54, 27, 38] # example of play list -certificatino: [101] \ No newline at end of file +certificatino: [101] diff --git a/op3_demo/package.xml b/op3_demo/package.xml index 1a0785c..9604a04 100644 --- a/op3_demo/package.xml +++ b/op3_demo/package.xml @@ -1,36 +1,80 @@ op3_demo - 0.1.0 + 0.3.0 OP3 default demo It includes three demontrations(soccer demo, vision demo, action script demo) Apache 2.0 Kayman - Pyo + Ronaldson Bellande + http://wiki.ros.org/op3_demo http://emanual.robotis.com/docs/en/platform/op3/introduction/ https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin - roscpp - roslib - std_msgs - sensor_msgs - geometry_msgs - robotis_controller_msgs - op3_walking_module_msgs - op3_action_module_msgs - cmake_modules - robotis_math - op3_ball_detector - boost - eigen - yaml-cpp + + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + robotis_controller_msgs + op3_walking_module_msgs + op3_action_module_msgs + cmake_modules + robotis_math + op3_ball_detector + boost + eigen + yaml-cpp + op3_manager + op3_camera_setting_tool + op3_web_setting_tool + ros_madplay_player + face_detection + + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + robotis_controller_msgs + op3_walking_module_msgs + op3_action_module_msgs + cmake_modules + robotis_math + op3_ball_detector + boost + eigen + yaml-cpp + op3_manager + op3_camera_setting_tool + op3_web_setting_tool + ros_madplay_player + face_detection + + roscpp + roslib + std_msgs + sensor_msgs + geometry_msgs + robotis_controller_msgs + op3_walking_module_msgs + op3_action_module_msgs + cmake_modules + robotis_math + op3_ball_detector + boost + eigen + yaml-cpp op3_manager op3_camera_setting_tool op3_web_setting_tool ros_madplay_player - + face_detection + diff --git a/op3_demo/src/action/action_demo.cpp b/op3_demo/src/action/action_demo.cpp index 660390c..30fbdda 100644 --- a/op3_demo/src/action/action_demo.cpp +++ b/op3_demo/src/action/action_demo.cpp @@ -1,56 +1,54 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_demo/action_demo.h" -namespace robotis_op -{ +namespace robotis_op { ActionDemo::ActionDemo() - : SPIN_RATE(30), - DEBUG_PRINT(false), - play_index_(0), - play_status_(StopAction) -{ + : SPIN_RATE(30), DEBUG_PRINT(false), play_index_(0), + play_status_(StopAction) { enable_ = false; ros::NodeHandle nh(ros::this_node::getName()); - std::string default_path = ros::package::getPath("op3_demo") + "/list/action_script.yaml"; + std::string default_path = + ros::package::getPath("op3_demo") + "/list/action_script.yaml"; script_path_ = nh.param("action_script", default_path); std::string default_play_list = "default"; - play_list_name_ = nh.param("action_script_play_list", default_play_list); + play_list_name_ = + nh.param("action_script_play_list", default_play_list); - demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &ActionDemo::demoCommandCallback, this); + demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, + &ActionDemo::demoCommandCallback, this); - parseActionScript (script_path_); + parseActionScript(script_path_); - boost::thread queue_thread = boost::thread(boost::bind(&ActionDemo::callbackThread, this)); - boost::thread process_thread = boost::thread(boost::bind(&ActionDemo::processThread, this)); + boost::thread queue_thread = + boost::thread(boost::bind(&ActionDemo::callbackThread, this)); + boost::thread process_thread = + boost::thread(boost::bind(&ActionDemo::processThread, this)); } -ActionDemo::~ActionDemo() -{ -} +ActionDemo::~ActionDemo() {} -void ActionDemo::setDemoEnable() -{ +void ActionDemo::setDemoEnable() { setModuleToDemo("action_module"); enable_ = true; @@ -62,8 +60,7 @@ void ActionDemo::setDemoEnable() startProcess(play_list_name_); } -void ActionDemo::setDemoDisable() -{ +void ActionDemo::setDemoDisable() { stopProcess(); enable_ = false; @@ -71,134 +68,116 @@ void ActionDemo::setDemoDisable() play_list_.resize(0); } -void ActionDemo::process() -{ - switch (play_status_) - { - case PlayAction: - { - if (play_list_.size() == 0) - { - ROS_WARN("Play List is empty."); - return; - } - - // action is not running - if (isActionRunning() == false) - { - // play - bool result_play = playActionWithSound(play_list_.at(play_index_)); - - ROS_INFO_COND(!result_play, "Fail to play action script."); - - // add play index - int index_to_play = (play_index_ + 1) % play_list_.size(); - play_index_ = index_to_play; - } - else - { - // wait - return; - } - break; +void ActionDemo::process() { + switch (play_status_) { + case PlayAction: { + if (play_list_.size() == 0) { + ROS_WARN("Play List is empty."); + return; } - case PauseAction: - { - stopMP3(); - brakeAction(); + // action is not running + if (isActionRunning() == false) { + // play + bool result_play = playActionWithSound(play_list_.at(play_index_)); - play_status_ = ReadyAction; + ROS_INFO_COND(!result_play, "Fail to play action script."); - break; + // add play index + int index_to_play = (play_index_ + 1) % play_list_.size(); + play_index_ = index_to_play; + } else { + // wait + return; } + break; + } - case StopAction: - { - stopMP3(); - stopAction(); + case PauseAction: { + stopMP3(); + brakeAction(); - play_status_ = ReadyAction; + play_status_ = ReadyAction; - break; - } + break; + } - default: - break; + case StopAction: { + stopMP3(); + stopAction(); + + play_status_ = ReadyAction; + + break; + } + + default: + break; } } -void ActionDemo::startProcess(const std::string &set_name) -{ +void ActionDemo::startProcess(const std::string &set_name) { parseActionScriptSetName(script_path_, set_name); play_status_ = PlayAction; } -void ActionDemo::resumeProcess() -{ - play_status_ = PlayAction; -} +void ActionDemo::resumeProcess() { play_status_ = PlayAction; } -void ActionDemo::pauseProcess() -{ - play_status_ = PauseAction; -} +void ActionDemo::pauseProcess() { play_status_ = PauseAction; } -void ActionDemo::stopProcess() -{ +void ActionDemo::stopProcess() { play_index_ = 0; play_status_ = StopAction; } -void ActionDemo::processThread() -{ - //set node loop rate +void ActionDemo::processThread() { + // set node loop rate ros::Rate loop_rate(SPIN_RATE); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { if (enable_ == true) process(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } } -void ActionDemo::callbackThread() -{ +void ActionDemo::callbackThread() { ros::NodeHandle nh(ros::this_node::getName()); // subscriber & publisher - module_control_pub_ = nh.advertise("/robotis/enable_ctrl_module", 0); - motion_index_pub_ = nh.advertise("/robotis/action/page_num", 0); + module_control_pub_ = + nh.advertise("/robotis/enable_ctrl_module", 0); + motion_index_pub_ = + nh.advertise("/robotis/action/page_num", 0); play_sound_pub_ = nh.advertise("/play_sound_file", 0); - buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &ActionDemo::buttonHandlerCallback, this); + buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, + &ActionDemo::buttonHandlerCallback, this); - is_running_client_ = nh.serviceClient("/robotis/action/is_running"); - set_joint_module_client_ = nh.serviceClient("/robotis/set_present_ctrl_modules"); + is_running_client_ = nh.serviceClient( + "/robotis/action/is_running"); + set_joint_module_client_ = + nh.serviceClient( + "/robotis/set_present_ctrl_modules"); - while (nh.ok()) - { + while (nh.ok()) { ros::spinOnce(); usleep(1000); } } -void ActionDemo::parseActionScript(const std::string &path) -{ +void ActionDemo::parseActionScript(const std::string &path) { YAML::Node doc; - try - { + try { // load yaml doc = YAML::LoadFile(path.c_str()); - } catch (const std::exception& e) - { + } catch (const std::exception &e) { ROS_ERROR_STREAM("Fail to load action script yaml. - " << e.what()); ROS_ERROR_STREAM("Script Path : " << path); return; @@ -206,8 +185,8 @@ void ActionDemo::parseActionScript(const std::string &path) // parse action_sound table YAML::Node sub_node = doc["action_and_sound"]; - for (YAML::iterator yaml_it = sub_node.begin(); yaml_it != sub_node.end(); ++yaml_it) - { + for (YAML::iterator yaml_it = sub_node.begin(); yaml_it != sub_node.end(); + ++yaml_it) { int action_index = yaml_it->first.as(); std::string mp3_path = yaml_it->second.as(); @@ -216,82 +195,75 @@ void ActionDemo::parseActionScript(const std::string &path) // default action set if (doc["default"]) - play_list_ = doc["default"].as >(); + play_list_ = doc["default"].as>(); } -bool ActionDemo::parseActionScriptSetName(const std::string &path, const std::string &set_name) -{ +bool ActionDemo::parseActionScriptSetName(const std::string &path, + const std::string &set_name) { YAML::Node doc; - try - { + try { // load yaml doc = YAML::LoadFile(path.c_str()); - } catch (const std::exception& e) - { + } catch (const std::exception &e) { ROS_ERROR("Fail to load yaml."); return false; } // parse action_sound table - if (doc[set_name]) - { - play_list_ = doc[set_name].as >(); + if (doc[set_name]) { + play_list_ = doc[set_name].as>(); return true; - } - else + } else return false; } -bool ActionDemo::playActionWithSound(int motion_index) -{ - std::map::iterator map_it = action_sound_table_.find(motion_index); +bool ActionDemo::playActionWithSound(int motion_index) { + std::map::iterator map_it = + action_sound_table_.find(motion_index); if (map_it == action_sound_table_.end()) return false; playAction(motion_index); playMP3(map_it->second); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "action : " << motion_index << ", mp3 path : " << map_it->second); + ROS_INFO_STREAM_COND(DEBUG_PRINT, + "action : " << motion_index + << ", mp3 path : " << map_it->second); return true; } -void ActionDemo::playMP3(std::string &path) -{ +void ActionDemo::playMP3(std::string &path) { std_msgs::String sound_msg; sound_msg.data = path; play_sound_pub_.publish(sound_msg); } -void ActionDemo::stopMP3() -{ +void ActionDemo::stopMP3() { std_msgs::String sound_msg; sound_msg.data = ""; play_sound_pub_.publish(sound_msg); } -void ActionDemo::playAction(int motion_index) -{ +void ActionDemo::playAction(int motion_index) { std_msgs::Int32 motion_msg; motion_msg.data = motion_index; motion_index_pub_.publish(motion_msg); } -void ActionDemo::stopAction() -{ +void ActionDemo::stopAction() { std_msgs::Int32 motion_msg; motion_msg.data = StopActionCommand; motion_index_pub_.publish(motion_msg); } -void ActionDemo::brakeAction() -{ +void ActionDemo::brakeAction() { std_msgs::Int32 motion_msg; motion_msg.data = BrakeActionCommand; @@ -299,19 +271,14 @@ void ActionDemo::brakeAction() } // check running of action -bool ActionDemo::isActionRunning() -{ +bool ActionDemo::isActionRunning() { op3_action_module_msgs::IsRunning is_running_srv; - if (is_running_client_.call(is_running_srv) == false) - { + if (is_running_client_.call(is_running_srv) == false) { ROS_ERROR("Failed to get action status"); return true; - } - else - { - if (is_running_srv.response.is_running == true) - { + } else { + if (is_running_srv.response.is_running == true) { return true; } } @@ -319,74 +286,58 @@ bool ActionDemo::isActionRunning() return false; } -void ActionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) -{ +void ActionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { if (enable_ == false) return; - if (msg->data == "start") - { - switch (play_status_) - { - case PlayAction: - { - pauseProcess(); - break; - } + if (msg->data == "start") { + switch (play_status_) { + case PlayAction: { + pauseProcess(); + break; + } - case PauseAction: - { - resumeProcess(); - break; - } + case PauseAction: { + resumeProcess(); + break; + } - case StopAction: - { - resumeProcess(); - break; - } - - default: - break; - } - } - else if (msg->data == "mode") - { + case StopAction: { + resumeProcess(); + break; + } + default: + break; + } + } else if (msg->data == "mode") { } } -void ActionDemo::setModuleToDemo(const std::string &module_name) -{ +void ActionDemo::setModuleToDemo(const std::string &module_name) { callServiceSettingModule(module_name); ROS_INFO_STREAM("enable module : " << module_name); } -void ActionDemo::callServiceSettingModule(const std::string &module_name) -{ - robotis_controller_msgs::SetModule set_module_srv; - set_module_srv.request.module_name = module_name; +void ActionDemo::callServiceSettingModule(const std::string &module_name) { + robotis_controller_msgs::SetModule set_module_srv; + set_module_srv.request.module_name = module_name; - if (set_joint_module_client_.call(set_module_srv) == false) - { - ROS_ERROR("Failed to set module"); - return; - } + if (set_joint_module_client_.call(set_module_srv) == false) { + ROS_ERROR("Failed to set module"); + return; + } - return ; + return; } -void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) -{ +void ActionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) { if (enable_ == false) return; - if (msg->data == "start") - { + if (msg->data == "start") { resumeProcess(); - } - else if (msg->data == "stop") - { + } else if (msg->data == "stop") { pauseProcess(); } } diff --git a/op3_demo/src/demo_node.cpp b/op3_demo/src/demo_node.cpp index 300a90b..90fb0a8 100644 --- a/op3_demo/src/demo_node.cpp +++ b/op3_demo/src/demo_node.cpp @@ -1,32 +1,31 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include #include -#include "op3_demo/soccer_demo.h" #include "op3_demo/action_demo.h" +#include "op3_demo/soccer_demo.h" #include "op3_demo/vision_demo.h" -#include "robotis_math/robotis_linear_algebra.h" #include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_math/robotis_linear_algebra.h" -enum Demo_Status -{ +enum Demo_Status { Ready = 0, SoccerDemo = 1, VisionDemo = 2, @@ -34,11 +33,11 @@ enum Demo_Status DemoCount = 4, }; -void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); +void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); void goInitPose(); void playSound(const std::string &path); void setLED(int led); -bool checkManagerRunning(std::string& manager_name); +bool checkManagerRunning(std::string &manager_name); void dxlTorqueChecker(); void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg); @@ -56,13 +55,12 @@ int current_status = Ready; int desired_status = Ready; bool apply_desired = false; -//node main -int main(int argc, char **argv) -{ - //init ros +// node main +int main(int argc, char **argv) { + // init ros ros::init(argc, argv, "demo_node"); - //create ros wrapper object + // create ros wrapper object robotis_op::OPDemo *current_demo = NULL; robotis_op::SoccerDemo *soccer_demo = new robotis_op::SoccerDemo(); robotis_op::ActionDemo *action_demo = new robotis_op::ActionDemo(); @@ -72,26 +70,27 @@ int main(int argc, char **argv) init_pose_pub = nh.advertise("/robotis/base/ini_pose", 0); play_sound_pub = nh.advertise("/play_sound_file", 0); - led_pub = nh.advertise("/robotis/sync_write_item", 0); + led_pub = nh.advertise( + "/robotis/sync_write_item", 0); dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); - ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); - ros::Subscriber mode_command_sub = nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback); + ros::Subscriber buttuon_sub = + nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); + ros::Subscriber mode_command_sub = + nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback); default_mp3_path = ros::package::getPath("op3_demo") + "/data/mp3/"; ros::start(); - //set node loop rate + // set node loop rate ros::Rate loop_rate(SPIN_RATE); // wait for starting of manager std::string manager_name = "/op3_manager"; - while (ros::ok()) - { + while (ros::ok()) { ros::Duration(1.0).sleep(); - if (checkManagerRunning(manager_name) == true) - { + if (checkManagerRunning(manager_name) == true) { break; ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect"); } @@ -103,164 +102,148 @@ int main(int argc, char **argv) // turn on R/G/B LED setLED(0x01 | 0x02 | 0x04); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { // process - if (apply_desired == true) - { - switch (desired_status) - { - case Ready: - { + if (apply_desired == true) { + switch (desired_status) { + case Ready: { - if (current_demo != NULL) - current_demo->setDemoDisable(); + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = NULL; + current_demo = NULL; - goInitPose(); + goInitPose(); - ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]"); - break; - } + ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]"); + break; + } - case SoccerDemo: - { - if (current_demo != NULL) - current_demo->setDemoDisable(); + case SoccerDemo: { + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = soccer_demo; - current_demo->setDemoEnable(); + current_demo = soccer_demo; + current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo"); - break; - } + ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo"); + break; + } - case VisionDemo: - { - if (current_demo != NULL) - current_demo->setDemoDisable(); + case VisionDemo: { + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = vision_demo; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo"); - break; - } - case ActionDemo: - { - if (current_demo != NULL) - current_demo->setDemoDisable(); + current_demo = vision_demo; + current_demo->setDemoEnable(); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo"); + break; + } + case ActionDemo: { + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = action_demo; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo"); - break; - } + current_demo = action_demo; + current_demo->setDemoEnable(); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo"); + break; + } - default: - { - break; - } + default: { + break; + } } apply_desired = false; current_status = desired_status; } - //execute pending callbacks + // execute pending callbacks ros::spinOnce(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } - //exit program + // exit program return 0; } -void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) -{ - if(apply_desired == true) +void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { + if (apply_desired == true) return; // in the middle of playing demo - if (current_status != Ready) - { - if (msg->data == "mode_long") - { + if (current_status != Ready) { + if (msg->data == "mode_long") { // go to mode selection status desired_status = Ready; apply_desired = true; playSound(default_mp3_path + "Demonstration ready mode.mp3"); setLED(0x01 | 0x02 | 0x04); - } - else if (msg->data == "user_long") - { + } else if (msg->data == "user_long") { // it's using in op3_manager // torque on and going to init pose } } // ready to start demo - else - { - if (msg->data == "start") - { + else { + if (msg->data == "start") { // select current demo - desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status; + desired_status = + (desired_status == Ready) ? desired_status + 1 : desired_status; apply_desired = true; // sound out desired status - switch (desired_status) - { - case SoccerDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start soccer demonstration.mp3"); - break; + switch (desired_status) { + case SoccerDemo: + dxlTorqueChecker(); + playSound(default_mp3_path + "Start soccer demonstration.mp3"); + break; - case VisionDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start vision processing demonstration.mp3"); - break; + case VisionDemo: + dxlTorqueChecker(); + playSound(default_mp3_path + + "Start vision processing demonstration.mp3"); + break; - case ActionDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start motion demonstration.mp3"); - break; + case ActionDemo: + dxlTorqueChecker(); + playSound(default_mp3_path + "Start motion demonstration.mp3"); + break; - default: - break; + default: + break; } ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } - else if (msg->data == "mode") - { + } else if (msg->data == "mode") { // change to next demo desired_status = (desired_status + 1) % DemoCount; - desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status; + desired_status = + (desired_status == Ready) ? desired_status + 1 : desired_status; // sound out desired status and changing LED - switch (desired_status) - { - case SoccerDemo: - playSound(default_mp3_path + "Autonomous soccer mode.mp3"); - setLED(0x01); - break; + switch (desired_status) { + case SoccerDemo: + playSound(default_mp3_path + "Autonomous soccer mode.mp3"); + setLED(0x01); + break; - case VisionDemo: - playSound(default_mp3_path + "Vision processing mode.mp3"); - setLED(0x02); - break; + case VisionDemo: + playSound(default_mp3_path + "Vision processing mode.mp3"); + setLED(0x02); + break; - case ActionDemo: - playSound(default_mp3_path + "Interactive motion mode.mp3"); - setLED(0x04); - break; + case ActionDemo: + playSound(default_mp3_path + "Interactive motion mode.mp3"); + setLED(0x04); + break; - default: - break; + default: + break; } ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status); @@ -268,24 +251,21 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) } } -void goInitPose() -{ +void goInitPose() { std_msgs::String init_msg; init_msg.data = "ini_pose"; init_pose_pub.publish(init_msg); } -void playSound(const std::string &path) -{ +void playSound(const std::string &path) { std_msgs::String sound_msg; sound_msg.data = path; play_sound_pub.publish(sound_msg); } -void setLED(int led) -{ +void setLED(int led) { robotis_controller_msgs::SyncWriteItem syncwrite_msg; syncwrite_msg.item_name = "LED"; syncwrite_msg.joint_name.push_back("open-cr"); @@ -294,13 +274,12 @@ void setLED(int led) led_pub.publish(syncwrite_msg); } -bool checkManagerRunning(std::string& manager_name) -{ +bool checkManagerRunning(std::string &manager_name) { std::vector node_list; ros::master::getNodes(node_list); - for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); node_list_idx++) - { + for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); + node_list_idx++) { if (node_list[node_list_idx] == manager_name) return true; } @@ -309,21 +288,17 @@ bool checkManagerRunning(std::string& manager_name) return false; } -void dxlTorqueChecker() -{ +void dxlTorqueChecker() { std_msgs::String check_msg; check_msg.data = "check"; dxl_torque_pub.publish(check_msg); } -void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) -{ +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) { // In demo mode - if (current_status != Ready) - { - if (msg->data == "ready") - { + if (current_status != Ready) { + if (msg->data == "ready") { // go to mode selection status desired_status = Ready; apply_desired = true; @@ -333,10 +308,8 @@ void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) } } // In ready mode - else - { - if(msg->data == "soccer") - { + else { + if (msg->data == "soccer") { desired_status = SoccerDemo; apply_desired = true; @@ -344,9 +317,7 @@ void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) dxlTorqueChecker(); playSound(default_mp3_path + "Start soccer demonstration.mp3"); ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } - else if(msg->data == "vision") - { + } else if (msg->data == "vision") { desired_status = VisionDemo; apply_desired = true; @@ -354,9 +325,7 @@ void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) dxlTorqueChecker(); playSound(default_mp3_path + "Start vision processing demonstration.mp3"); ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } - else if(msg->data == "action") - { + } else if (msg->data == "action") { desired_status = ActionDemo; apply_desired = true; @@ -367,4 +336,3 @@ void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) } } } - diff --git a/op3_demo/src/soccer/ball_follower.cpp b/op3_demo/src/soccer/ball_follower.cpp index fdd0d20..f4ef1e0 100644 --- a/op3_demo/src/soccer/ball_follower.cpp +++ b/op3_demo/src/soccer/ball_follower.cpp @@ -1,117 +1,91 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_demo/ball_follower.h" -namespace robotis_op -{ +namespace robotis_op { BallFollower::BallFollower() - : nh_(ros::this_node::getName()), - FOV_WIDTH(35.2 * M_PI / 180), - FOV_HEIGHT(21.6 * M_PI / 180), - count_not_found_(0), - count_to_kick_(0), - on_tracking_(false), - approach_ball_position_(NotFound), - kick_motion_index_(83), - CAMERA_HEIGHT(0.46), - NOT_FOUND_THRESHOLD(50), - MAX_FB_STEP(40.0 * 0.001), - MAX_RL_TURN(15.0 * M_PI / 180), - IN_PLACE_FB_STEP(-3.0 * 0.001), - MIN_FB_STEP(5.0 * 0.001), - MIN_RL_TURN(5.0 * M_PI / 180), - UNIT_FB_STEP(1.0 * 0.001), - UNIT_RL_TURN(0.5 * M_PI / 180), - SPOT_FB_OFFSET(0.0 * 0.001), - SPOT_RL_OFFSET(0.0 * 0.001), - SPOT_ANGLE_OFFSET(0.0), - hip_pitch_offset_(7.0), - current_pan_(-10), - current_tilt_(-10), - current_x_move_(0.005), - current_r_angle_(0), - curr_period_time_(0.6), - accum_period_time_(0.0), - DEBUG_PRINT(false) -{ - current_joint_states_sub_ = nh_.subscribe("/robotis/goal_joint_states", 10, &BallFollower::currentJointStatesCallback, - this); + : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180), + FOV_HEIGHT(21.6 * M_PI / 180), count_not_found_(0), count_to_kick_(0), + on_tracking_(false), approach_ball_position_(NotFound), + kick_motion_index_(83), CAMERA_HEIGHT(0.46), NOT_FOUND_THRESHOLD(50), + MAX_FB_STEP(40.0 * 0.001), MAX_RL_TURN(15.0 * M_PI / 180), + IN_PLACE_FB_STEP(-3.0 * 0.001), MIN_FB_STEP(5.0 * 0.001), + MIN_RL_TURN(5.0 * M_PI / 180), UNIT_FB_STEP(1.0 * 0.001), + UNIT_RL_TURN(0.5 * M_PI / 180), SPOT_FB_OFFSET(0.0 * 0.001), + SPOT_RL_OFFSET(0.0 * 0.001), SPOT_ANGLE_OFFSET(0.0), + hip_pitch_offset_(7.0), current_pan_(-10), current_tilt_(-10), + current_x_move_(0.005), current_r_angle_(0), curr_period_time_(0.6), + accum_period_time_(0.0), DEBUG_PRINT(false) { + current_joint_states_sub_ = + nh_.subscribe("/robotis/goal_joint_states", 10, + &BallFollower::currentJointStatesCallback, this); - set_walking_command_pub_ = nh_.advertise("/robotis/walking/command", 0); - set_walking_param_pub_ = nh_.advertise("/robotis/walking/set_params", 0); - get_walking_param_client_ = nh_.serviceClient( - "/robotis/walking/get_params"); + set_walking_command_pub_ = + nh_.advertise("/robotis/walking/command", 0); + set_walking_param_pub_ = nh_.advertise( + "/robotis/walking/set_params", 0); + get_walking_param_client_ = + nh_.serviceClient( + "/robotis/walking/get_params"); prev_time_ = ros::Time::now(); } -BallFollower::~BallFollower() -{ +BallFollower::~BallFollower() {} -} - -void BallFollower::startFollowing() -{ +void BallFollower::startFollowing() { on_tracking_ = true; ROS_INFO("Start Ball following"); setWalkingCommand("start"); bool result = getWalkingParam(); - if (result == true) - { + if (result == true) { hip_pitch_offset_ = current_walking_param_.hip_pitch_offset; curr_period_time_ = current_walking_param_.period_time; - } - else - { + } else { hip_pitch_offset_ = 7.0 * M_PI / 180; curr_period_time_ = 0.6; } } -void BallFollower::stopFollowing() -{ +void BallFollower::stopFollowing() { on_tracking_ = false; // approach_ball_position_ = NotFound; count_to_kick_ = 0; -// accum_ball_position_ = 0; + // accum_ball_position_ = 0; ROS_INFO("Stop Ball following"); setWalkingCommand("stop"); } -void BallFollower::currentJointStatesCallback(const sensor_msgs::JointState::ConstPtr &msg) -{ +void BallFollower::currentJointStatesCallback( + const sensor_msgs::JointState::ConstPtr &msg) { double pan, tilt; int get_count = 0; - for (int ix = 0; ix < msg->name.size(); ix++) - { - if (msg->name[ix] == "head_pan") - { + for (int ix = 0; ix < msg->name.size(); ix++) { + if (msg->name[ix] == "head_pan") { pan = msg->position[ix]; get_count += 1; - } - else if (msg->name[ix] == "head_tilt") - { + } else if (msg->name[ix] == "head_tilt") { tilt = msg->position[ix]; get_count += 1; } @@ -125,9 +99,9 @@ void BallFollower::currentJointStatesCallback(const sensor_msgs::JointState::Con current_tilt_ = tilt; } -void BallFollower::calcFootstep(double target_distance, double target_angle, double delta_time, - double& fb_move, double& rl_angle) -{ +void BallFollower::calcFootstep(double target_distance, double target_angle, + double delta_time, double &fb_move, + double &rl_angle) { // clac fb double next_movement = current_x_move_; if (target_distance < 0) @@ -135,8 +109,7 @@ void BallFollower::calcFootstep(double target_distance, double target_angle, dou double fb_goal = fmin(target_distance * 0.1, MAX_FB_STEP); accum_period_time_ += delta_time; - if (accum_period_time_ > (curr_period_time_ / 4)) - { + if (accum_period_time_ > (curr_period_time_ / 4)) { accum_period_time_ = 0.0; if ((target_distance * 0.1 / 2) < current_x_move_) next_movement -= UNIT_FB_STEP; @@ -145,14 +118,14 @@ void BallFollower::calcFootstep(double target_distance, double target_angle, dou } fb_goal = fmin(next_movement, fb_goal); fb_move = fmax(fb_goal, MIN_FB_STEP); - ROS_INFO_COND(DEBUG_PRINT, "distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", target_distance, fb_move, - delta_time); + ROS_INFO_COND(DEBUG_PRINT, + "distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", + target_distance, fb_move, delta_time); ROS_INFO_COND(DEBUG_PRINT, "=============================================="); // calc rl angle double rl_goal = 0.0; - if (fabs(target_angle) * 180 / M_PI > 5.0) - { + if (fabs(target_angle) * 180 / M_PI > 5.0) { double rl_offset = fabs(target_angle) * 0.2; rl_goal = fmin(rl_offset, MAX_RL_TURN); rl_goal = fmax(rl_goal, MIN_RL_TURN); @@ -163,20 +136,20 @@ void BallFollower::calcFootstep(double target_distance, double target_angle, dou } } -// x_angle : ball position (pan), y_angle : ball position (tilt), ball_size : angle of ball radius -bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_size) -{ +// x_angle : ball position (pan), y_angle : ball position (tilt), ball_size : +// angle of ball radius +bool BallFollower::processFollowing(double x_angle, double y_angle, + double ball_size) { ros::Time curr_time = ros::Time::now(); ros::Duration dur = curr_time - prev_time_; double delta_time = dur.nsec * 0.000000001 + dur.sec; prev_time_ = curr_time; count_not_found_ = 0; -// int ball_position_sum = 0; + // int ball_position_sum = 0; // check of getting head joints angle - if (current_tilt_ == -10 && current_pan_ == -10) - { + if (current_tilt_ == -10 && current_pan_ == -10) { ROS_ERROR("Failed to get current angle of head joints."); setWalkingCommand("stop"); @@ -187,13 +160,18 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ ROS_INFO_COND(DEBUG_PRINT, " ============== Head | Ball ============== "); ROS_INFO_STREAM_COND(DEBUG_PRINT, - "== Head Pan : " << (current_pan_ * 180 / M_PI) << " | Ball X : " << (x_angle * 180 / M_PI)); + "== Head Pan : " << (current_pan_ * 180 / M_PI) + << " | Ball X : " + << (x_angle * 180 / M_PI)); ROS_INFO_STREAM_COND(DEBUG_PRINT, - "== Head Tilt : " << (current_tilt_ * 180 / M_PI) << " | Ball Y : " << (y_angle * 180 / M_PI)); + "== Head Tilt : " << (current_tilt_ * 180 / M_PI) + << " | Ball Y : " + << (y_angle * 180 / M_PI)); approach_ball_position_ = OutOfRange; - double distance_to_ball = CAMERA_HEIGHT * tan(M_PI * 0.5 + current_tilt_ - hip_pitch_offset_ - ball_size); + double distance_to_ball = CAMERA_HEIGHT * tan(M_PI * 0.5 + current_tilt_ - + hip_pitch_offset_ - ball_size); double ball_y_angle = (current_tilt_ + y_angle) * 180 / M_PI; double ball_x_angle = (current_pan_ + x_angle) * 180 / M_PI; @@ -201,53 +179,51 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ if (distance_to_ball < 0) distance_to_ball *= (-1); - //double distance_to_kick = 0.25; + // double distance_to_kick = 0.25; double distance_to_kick = 0.22; // check whether ball is correct position. - if ((distance_to_ball < distance_to_kick) && (fabs(ball_x_angle) < 25.0)) - { + if ((distance_to_ball < distance_to_kick) && (fabs(ball_x_angle) < 25.0)) { count_to_kick_ += 1; ROS_INFO_STREAM_COND(DEBUG_PRINT, - "head pan : " << (current_pan_ * 180 / M_PI) << " | ball pan : " << (x_angle * 180 / M_PI)); + "head pan : " << (current_pan_ * 180 / M_PI) + << " | ball pan : " + << (x_angle * 180 / M_PI)); ROS_INFO_STREAM_COND(DEBUG_PRINT, - "head tilt : " << (current_tilt_ * 180 / M_PI) << " | ball tilt : " << (y_angle * 180 / M_PI)); + "head tilt : " << (current_tilt_ * 180 / M_PI) + << " | ball tilt : " + << (y_angle * 180 / M_PI)); ROS_INFO_STREAM_COND(DEBUG_PRINT, "foot to kick : " << ball_x_angle); ROS_INFO("In range [%d | %d]", count_to_kick_, ball_x_angle); // ball queue -// if(ball_position_queue_.size() >= 5) -// ball_position_queue_.erase(ball_position_queue_.begin()); + // if(ball_position_queue_.size() >= 5) + // ball_position_queue_.erase(ball_position_queue_.begin()); -// ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1); + // ball_position_queue_.push_back((ball_x_angle > 0) ? 1 : -1); - - if (count_to_kick_ > 20) - { + if (count_to_kick_ > 20) { setWalkingCommand("stop"); on_tracking_ = false; // check direction of the ball -// accum_ball_position_ = std::accumulate(ball_position_queue_.begin(), ball_position_queue_.end(), 0); + // accum_ball_position_ = + // std::accumulate(ball_position_queue_.begin(), + // ball_position_queue_.end(), 0); -// if (accum_ball_position_ > 0) - if (ball_x_angle > 0) - { - ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left"); // left + // if (accum_ball_position_ > 0) + if (ball_x_angle > 0) { + ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : left"); // left approach_ball_position_ = OnLeft; - } - else - { - ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : right"); // right + } else { + ROS_INFO_COND(DEBUG_PRINT, "Ready to kick : right"); // right approach_ball_position_ = OnRight; } return true; - } - else if (count_to_kick_ > 15) - { + } else if (count_to_kick_ > 15) { // if (ball_x_angle > 0) // accum_ball_position_ += 1; // else @@ -258,11 +234,9 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ return false; } - } - else - { + } else { count_to_kick_ = 0; -// accum_ball_position_ = NotFound; + // accum_ball_position_ = NotFound; } double fb_move = 0.0, rl_angle = 0.0; @@ -274,16 +248,15 @@ bool BallFollower::processFollowing(double x_angle, double y_angle, double ball_ setWalkingParam(fb_move, 0, rl_angle); // for debug - //ROS_INFO("distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", distance_to_ball, fb_move, delta_time); + // ROS_INFO("distance to ball : %6.4f, fb : %6.4f, delta : %6.6f", + // distance_to_ball, fb_move, delta_time); return false; } -void BallFollower::decideBallPositin(double x_angle, double y_angle) -{ +void BallFollower::decideBallPositin(double x_angle, double y_angle) { // check of getting head joints angle - if (current_tilt_ == -10 && current_pan_ == -10) - { + if (current_tilt_ == -10 && current_pan_ == -10) { approach_ball_position_ = NotFound; return; } @@ -296,19 +269,16 @@ void BallFollower::decideBallPositin(double x_angle, double y_angle) approach_ball_position_ = OnRight; } -void BallFollower::waitFollowing() -{ +void BallFollower::waitFollowing() { count_not_found_++; if (count_not_found_ > NOT_FOUND_THRESHOLD * 0.5) setWalkingParam(0.0, 0.0, 0.0); } -void BallFollower::setWalkingCommand(const std::string &command) -{ +void BallFollower::setWalkingCommand(const std::string &command) { // get param - if (command == "start") - { + if (command == "start") { getWalkingParam(); setWalkingParam(IN_PLACE_FB_STEP, 0, 0, true); } @@ -320,12 +290,13 @@ void BallFollower::setWalkingCommand(const std::string &command) ROS_INFO_STREAM_COND(DEBUG_PRINT, "Send Walking command : " << command); } -void BallFollower::setWalkingParam(double x_move, double y_move, double rotation_angle, bool balance) -{ +void BallFollower::setWalkingParam(double x_move, double y_move, + double rotation_angle, bool balance) { current_walking_param_.balance_enable = balance; current_walking_param_.x_move_amplitude = x_move + SPOT_FB_OFFSET; current_walking_param_.y_move_amplitude = y_move + SPOT_RL_OFFSET; - current_walking_param_.angle_move_amplitude = rotation_angle + SPOT_ANGLE_OFFSET; + current_walking_param_.angle_move_amplitude = + rotation_angle + SPOT_ANGLE_OFFSET; set_walking_param_pub_.publish(current_walking_param_); @@ -333,27 +304,21 @@ void BallFollower::setWalkingParam(double x_move, double y_move, double rotation current_r_angle_ = rotation_angle; } -bool BallFollower::getWalkingParam() -{ +bool BallFollower::getWalkingParam() { op3_walking_module_msgs::GetWalkingParam walking_param_msg; - if (get_walking_param_client_.call(walking_param_msg)) - { + if (get_walking_param_client_.call(walking_param_msg)) { current_walking_param_ = walking_param_msg.response.parameters; // update ui ROS_INFO_COND(DEBUG_PRINT, "Get walking parameters"); return true; - } - else - { + } else { ROS_ERROR("Fail to get walking parameters."); return false; } - -} - } +} // namespace robotis_op diff --git a/op3_demo/src/soccer/ball_tracker.cpp b/op3_demo/src/soccer/ball_tracker.cpp index a1706ad..780613c 100644 --- a/op3_demo/src/soccer/ball_tracker.cpp +++ b/op3_demo/src/soccer/ball_tracker.cpp @@ -1,69 +1,61 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_demo/ball_tracker.h" -namespace robotis_op -{ +namespace robotis_op { BallTracker::BallTracker() - : nh_(ros::this_node::getName()), - FOV_WIDTH(35.2 * M_PI / 180), - FOV_HEIGHT(21.6 * M_PI / 180), - NOT_FOUND_THRESHOLD(50), - WAITING_THRESHOLD(5), - use_head_scan_(true), - count_not_found_(0), - on_tracking_(false), - current_ball_pan_(0), - current_ball_tilt_(0), - x_error_sum_(0), - y_error_sum_(0), - current_ball_bottom_(0), - tracking_status_(NotFound), - DEBUG_PRINT(false) -{ + : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180), + FOV_HEIGHT(21.6 * M_PI / 180), NOT_FOUND_THRESHOLD(50), + WAITING_THRESHOLD(5), use_head_scan_(true), count_not_found_(0), + on_tracking_(false), current_ball_pan_(0), current_ball_tilt_(0), + x_error_sum_(0), y_error_sum_(0), current_ball_bottom_(0), + tracking_status_(NotFound), DEBUG_PRINT(false) { ros::NodeHandle param_nh("~"); p_gain_ = param_nh.param("p_gain", 0.4); i_gain_ = param_nh.param("i_gain", 0.0); d_gain_ = param_nh.param("d_gain", 0.0); - ROS_INFO_STREAM("Ball tracking Gain : " << p_gain_ << ", " << i_gain_ << ", " << d_gain_); + ROS_INFO_STREAM("Ball tracking Gain : " << p_gain_ << ", " << i_gain_ << ", " + << d_gain_); - head_joint_offset_pub_ = nh_.advertise("/robotis/head_control/set_joint_states_offset", 0); - head_joint_pub_ = nh_.advertise("/robotis/head_control/set_joint_states", 0); - head_scan_pub_ = nh_.advertise("/robotis/head_control/scan_command", 0); - // error_pub_ = nh_.advertise("/ball_tracker/errors", 0); + head_joint_offset_pub_ = nh_.advertise( + "/robotis/head_control/set_joint_states_offset", 0); + head_joint_pub_ = nh_.advertise( + "/robotis/head_control/set_joint_states", 0); + head_scan_pub_ = + nh_.advertise("/robotis/head_control/scan_command", 0); + // error_pub_ = + // nh_.advertise("/ball_tracker/errors", 0); - ball_position_sub_ = nh_.subscribe("/ball_detector_node/circle_set", 1, &BallTracker::ballPositionCallback, this); - ball_tracking_command_sub_ = nh_.subscribe("/ball_tracker/command", 1, &BallTracker::ballTrackerCommandCallback, - this); + ball_position_sub_ = nh_.subscribe("/ball_detector_node/circle_set", 1, + &BallTracker::ballPositionCallback, this); + ball_tracking_command_sub_ = + nh_.subscribe("/ball_tracker/command", 1, + &BallTracker::ballTrackerCommandCallback, this); } -BallTracker::~BallTracker() -{ +BallTracker::~BallTracker() {} -} - -void BallTracker::ballPositionCallback(const op3_ball_detector::CircleSetStamped::ConstPtr &msg) -{ - for (int idx = 0; idx < msg->circles.size(); idx++) - { +void BallTracker::ballPositionCallback( + const op3_ball_detector::CircleSetStamped::ConstPtr &msg) { + for (int idx = 0; idx < msg->circles.size(); idx++) { if (ball_position_.z >= msg->circles[idx].z) continue; @@ -71,18 +63,13 @@ void BallTracker::ballPositionCallback(const op3_ball_detector::CircleSetStamped } } -void BallTracker::ballTrackerCommandCallback(const std_msgs::String::ConstPtr &msg) -{ - if (msg->data == "start") - { +void BallTracker::ballTrackerCommandCallback( + const std_msgs::String::ConstPtr &msg) { + if (msg->data == "start") { startTracking(); - } - else if (msg->data == "stop") - { + } else if (msg->data == "stop") { stopTracking(); - } - else if (msg->data == "toggle_start") - { + } else if (msg->data == "toggle_start") { if (on_tracking_ == false) startTracking(); else @@ -90,14 +77,12 @@ void BallTracker::ballTrackerCommandCallback(const std_msgs::String::ConstPtr &m } } -void BallTracker::startTracking() -{ +void BallTracker::startTracking() { on_tracking_ = true; ROS_INFO_COND(DEBUG_PRINT, "Start Ball tracking"); } -void BallTracker::stopTracking() -{ +void BallTracker::stopTracking() { goInit(); on_tracking_ = false; @@ -109,47 +94,34 @@ void BallTracker::stopTracking() y_error_sum_ = 0; } -void BallTracker::setUsingHeadScan(bool use_scan) -{ - use_head_scan_ = use_scan; -} +void BallTracker::setUsingHeadScan(bool use_scan) { use_head_scan_ = use_scan; } -int BallTracker::processTracking() -{ +int BallTracker::processTracking() { int tracking_status = Found; - if (on_tracking_ == false) - { + if (on_tracking_ == false) { ball_position_.z = 0; count_not_found_ = 0; return NotFound; } // check ball position - if (ball_position_.z <= 0) - { + if (ball_position_.z <= 0) { count_not_found_++; - if (count_not_found_ < WAITING_THRESHOLD) - { - if(tracking_status_ == Found || tracking_status_ == Waiting) + if (count_not_found_ < WAITING_THRESHOLD) { + if (tracking_status_ == Found || tracking_status_ == Waiting) tracking_status = Waiting; else tracking_status = NotFound; - } - else if (count_not_found_ > NOT_FOUND_THRESHOLD) - { + } else if (count_not_found_ > NOT_FOUND_THRESHOLD) { scanBall(); count_not_found_ = 0; tracking_status = NotFound; - } - else - { + } else { tracking_status = NotFound; } - } - else - { + } else { count_not_found_ = 0; } @@ -159,8 +131,7 @@ int BallTracker::processTracking() // offset_rad : top-left(+, +), bottom-right(-, -) double x_error = 0.0, y_error = 0.0, ball_size = 0.0; - switch (tracking_status) - { + switch (tracking_status) { case NotFound: tracking_status_ = tracking_status; current_ball_pan_ = 0; @@ -183,9 +154,15 @@ int BallTracker::processTracking() break; } - ROS_INFO_STREAM_COND(DEBUG_PRINT, "--------------------------------------------------------------"); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "Ball position : " << ball_position_.x << " | " << ball_position_.y); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "Target angle : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI)); + ROS_INFO_STREAM_COND( + DEBUG_PRINT, + "--------------------------------------------------------------"); + ROS_INFO_STREAM_COND(DEBUG_PRINT, "Ball position : " << ball_position_.x + << " | " + << ball_position_.y); + ROS_INFO_STREAM_COND(DEBUG_PRINT, + "Target angle : " << (x_error * 180 / M_PI) << " | " + << (y_error * 180 / M_PI)); ros::Time curr_time = ros::Time::now(); ros::Duration dur = curr_time - prev_time_; @@ -196,8 +173,10 @@ int BallTracker::processTracking() double y_error_diff = (y_error - current_ball_tilt_) / delta_time; x_error_sum_ += x_error; y_error_sum_ += y_error; - double x_error_target = x_error * p_gain_ + x_error_diff * d_gain_ + x_error_sum_ * i_gain_; - double y_error_target = y_error * p_gain_ + y_error_diff * d_gain_ + y_error_sum_ * i_gain_; + double x_error_target = + x_error * p_gain_ + x_error_diff * d_gain_ + x_error_sum_ * i_gain_; + double y_error_target = + y_error * p_gain_ + y_error_diff * d_gain_ + y_error_sum_ * i_gain_; // std_msgs::Float64MultiArray x_error_msg; // x_error_msg.data.push_back(x_error); @@ -209,17 +188,25 @@ int BallTracker::processTracking() // x_error_msg.data.push_back(x_error_target); // error_pub_.publish(x_error_msg); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "------------------------ " << tracking_status << " --------------------------------------"); - ROS_INFO_STREAM_COND(DEBUG_PRINT, "error : " << (x_error * 180 / M_PI) << " | " << (y_error * 180 / M_PI)); + ROS_INFO_STREAM_COND(DEBUG_PRINT, + "------------------------ " + << tracking_status + << " --------------------------------------"); + ROS_INFO_STREAM_COND(DEBUG_PRINT, + "error : " << (x_error * 180 / M_PI) << " | " + << (y_error * 180 / M_PI)); + ROS_INFO_STREAM_COND(DEBUG_PRINT, "error_diff : " + << (x_error_diff * 180 / M_PI) << " | " + << (y_error_diff * 180 / M_PI) << " | " + << delta_time); + ROS_INFO_STREAM_COND(DEBUG_PRINT, + "error_sum : " << (x_error_sum_ * 180 / M_PI) << " | " + << (y_error_sum_ * 180 / M_PI)); ROS_INFO_STREAM_COND( - DEBUG_PRINT, - "error_diff : " << (x_error_diff * 180 / M_PI) << " | " << (y_error_diff * 180 / M_PI) << " | " << delta_time); - ROS_INFO_STREAM_COND( - DEBUG_PRINT, - "error_sum : " << (x_error_sum_ * 180 / M_PI) << " | " << (y_error_sum_ * 180 / M_PI)); - ROS_INFO_STREAM_COND( - DEBUG_PRINT, - "error_target : " << (x_error_target * 180 / M_PI) << " | " << (y_error_target * 180 / M_PI) << " | P : " << p_gain_ << " | D : " << d_gain_ << " | time : " << delta_time); + DEBUG_PRINT, "error_target : " << (x_error_target * 180 / M_PI) << " | " + << (y_error_target * 180 / M_PI) + << " | P : " << p_gain_ << " | D : " + << d_gain_ << " | time : " << delta_time); // move head joint publishHeadJoint(x_error_target, y_error_target); @@ -235,8 +222,7 @@ int BallTracker::processTracking() return tracking_status; } -void BallTracker::publishHeadJoint(double pan, double tilt) -{ +void BallTracker::publishHeadJoint(double pan, double tilt) { double min_angle = 1 * M_PI / 180; if (fabs(pan) < min_angle && fabs(tilt) < min_angle) return; @@ -252,8 +238,7 @@ void BallTracker::publishHeadJoint(double pan, double tilt) head_joint_offset_pub_.publish(head_angle_msg); } -void BallTracker::goInit() -{ +void BallTracker::goInit() { sensor_msgs::JointState head_angle_msg; head_angle_msg.name.push_back("head_pan"); @@ -265,8 +250,7 @@ void BallTracker::goInit() head_joint_pub_.publish(head_angle_msg); } -void BallTracker::scanBall() -{ +void BallTracker::scanBall() { if (use_head_scan_ == false) return; @@ -280,5 +264,4 @@ void BallTracker::scanBall() head_scan_pub_.publish(scan_msg); } -} - +} // namespace robotis_op diff --git a/op3_demo/src/soccer/soccer_demo.cpp b/op3_demo/src/soccer/soccer_demo.cpp index 6389e15..3951b28 100644 --- a/op3_demo/src/soccer/soccer_demo.cpp +++ b/op3_demo/src/soccer/soccer_demo.cpp @@ -1,73 +1,61 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_demo/soccer_demo.h" -namespace robotis_op -{ +namespace robotis_op { SoccerDemo::SoccerDemo() - : FALL_FORWARD_LIMIT(60), - FALL_BACK_LIMIT(-60), - SPIN_RATE(30), - DEBUG_PRINT(false), - wait_count_(0), - on_following_ball_(false), - on_tracking_ball_(false), - restart_soccer_(false), - start_following_(false), - stop_following_(false), - stop_fallen_check_(false), - robot_status_(Waited), - stand_state_(Stand), - tracking_status_(BallTracker::Waiting), - present_pitch_(0) -{ - //init ros + : FALL_FORWARD_LIMIT(60), FALL_BACK_LIMIT(-60), SPIN_RATE(30), + DEBUG_PRINT(false), wait_count_(0), on_following_ball_(false), + on_tracking_ball_(false), restart_soccer_(false), start_following_(false), + stop_following_(false), stop_fallen_check_(false), robot_status_(Waited), + stand_state_(Stand), tracking_status_(BallTracker::Waiting), + present_pitch_(0) { + // init ros enable_ = false; ros::NodeHandle nh(ros::this_node::getName()); - std::string default_path = ros::package::getPath("op3_gui_demo") + "/config/gui_config.yaml"; + std::string default_path = + ros::package::getPath("op3_gui_demo") + "/config/gui_config.yaml"; std::string path = nh.param("demo_config", default_path); parseJointNameFromYaml(path); - boost::thread queue_thread = boost::thread(boost::bind(&SoccerDemo::callbackThread, this)); - boost::thread process_thread = boost::thread(boost::bind(&SoccerDemo::processThread, this)); - boost::thread tracking_thread = boost::thread(boost::bind(&SoccerDemo::trackingThread, this)); + boost::thread queue_thread = + boost::thread(boost::bind(&SoccerDemo::callbackThread, this)); + boost::thread process_thread = + boost::thread(boost::bind(&SoccerDemo::processThread, this)); + boost::thread tracking_thread = + boost::thread(boost::bind(&SoccerDemo::trackingThread, this)); is_grass_ = nh.param("grass_demo", false); } -SoccerDemo::~SoccerDemo() -{ +SoccerDemo::~SoccerDemo() {} -} - -void SoccerDemo::setDemoEnable() -{ +void SoccerDemo::setDemoEnable() { enable_ = true; startSoccerMode(); } -void SoccerDemo::setDemoDisable() -{ +void SoccerDemo::setDemoDisable() { // handle disable procedure ball_tracker_.stopTracking(); ball_follower_.stopFollowing(); @@ -84,14 +72,12 @@ void SoccerDemo::setDemoDisable() tracking_status_ = BallTracker::Waiting; } -void SoccerDemo::process() -{ - if(enable_ == false) +void SoccerDemo::process() { + if (enable_ == false) return; // check to start - if (start_following_ == true) - { + if (start_following_ == true) { ball_tracker_.startTracking(); ball_follower_.startFollowing(); start_following_ = false; @@ -100,8 +86,7 @@ void SoccerDemo::process() } // check to stop - if (stop_following_ == true) - { + if (stop_following_ == true) { ball_tracker_.stopTracking(); ball_follower_.stopFollowing(); stop_following_ = false; @@ -109,15 +94,13 @@ void SoccerDemo::process() wait_count_ = 0; } - if (wait_count_ <= 0) - { + if (wait_count_ <= 0) { // ball following - if (on_following_ball_ == true) - { - switch(tracking_status_) - { + if (on_following_ball_ == true) { + switch (tracking_status_) { case BallTracker::Found: - ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall(), 0.0); + ball_follower_.processFollowing(ball_tracker_.getPanOfBall(), + ball_tracker_.getTiltOfBall(), 0.0); break; case BallTracker::NotFound: @@ -130,119 +113,113 @@ void SoccerDemo::process() } // check fallen states - switch (stand_state_) - { - case Stand: - { + switch (stand_state_) { + case Stand: { // check restart soccer - if (restart_soccer_ == true) - { + if (restart_soccer_ == true) { restart_soccer_ = false; startSoccerMode(); break; } // check states for kick -// int ball_position = ball_follower_.getBallPosition(); + // int ball_position = ball_follower_.getBallPosition(); bool in_range = ball_follower_.isBallInRange(); - if(in_range == true) - { + if (in_range == true) { ball_follower_.stopFollowing(); handleKick(); } break; } // fallen state : Fallen_Forward, Fallen_Behind - default: - { + default: { ball_follower_.stopFollowing(); handleFallen(stand_state_); break; } } - } - else - { + } else { wait_count_ -= 1; } } -void SoccerDemo::processThread() -{ +void SoccerDemo::processThread() { bool result = false; - //set node loop rate + // set node loop rate ros::Rate loop_rate(SPIN_RATE); ball_tracker_.startTracking(); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { if (enable_ == true) process(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } } -void SoccerDemo::callbackThread() -{ +void SoccerDemo::callbackThread() { ros::NodeHandle nh(ros::this_node::getName()); // subscriber & publisher - module_control_pub_ = nh.advertise("/robotis/set_joint_ctrl_modules", 0); - motion_index_pub_ = nh.advertise("/robotis/action/page_num", 0); - rgb_led_pub_ = nh.advertise("/robotis/sync_write_item", 0); + module_control_pub_ = nh.advertise( + "/robotis/set_joint_ctrl_modules", 0); + motion_index_pub_ = + nh.advertise("/robotis/action/page_num", 0); + rgb_led_pub_ = nh.advertise( + "/robotis/sync_write_item", 0); - buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &SoccerDemo::buttonHandlerCallback, this); - demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, &SoccerDemo::demoCommandCallback, this); - imu_data_sub_ = nh.subscribe("/robotis/open_cr/imu", 1, &SoccerDemo::imuDataCallback, this); + buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, + &SoccerDemo::buttonHandlerCallback, this); + demo_command_sub_ = nh.subscribe("/robotis/demo_command", 1, + &SoccerDemo::demoCommandCallback, this); + imu_data_sub_ = nh.subscribe("/robotis/open_cr/imu", 1, + &SoccerDemo::imuDataCallback, this); - is_running_client_ = nh.serviceClient("/robotis/action/is_running"); - set_joint_module_client_ = nh.serviceClient("/robotis/set_present_joint_ctrl_modules"); + is_running_client_ = nh.serviceClient( + "/robotis/action/is_running"); + set_joint_module_client_ = + nh.serviceClient( + "/robotis/set_present_joint_ctrl_modules"); test_pub_ = nh.advertise("/debug_text", 0); - while (nh.ok()) - { + while (nh.ok()) { ros::spinOnce(); usleep(1000); } } -void SoccerDemo::trackingThread() -{ +void SoccerDemo::trackingThread() { - //set node loop rate + // set node loop rate ros::Rate loop_rate(SPIN_RATE); ball_tracker_.startTracking(); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { - if(enable_ == true && on_tracking_ball_ == true) - { + if (enable_ == true && on_tracking_ball_ == true) { // ball tracking int tracking_status; tracking_status = ball_tracker_.processTracking(); // set led - switch(tracking_status) - { + switch (tracking_status) { case BallTracker::Found: - if(tracking_status_ != tracking_status) + if (tracking_status_ != tracking_status) setRGBLED(0x1F, 0x1F, 0x1F); break; case BallTracker::NotFound: - if(tracking_status_ != tracking_status) + if (tracking_status_ != tracking_status) setRGBLED(0, 0, 0); break; @@ -250,36 +227,31 @@ void SoccerDemo::trackingThread() break; } - if(tracking_status != tracking_status_) + if (tracking_status != tracking_status_) tracking_status_ = tracking_status; } - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } } -void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_head_control) -{ +void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, + bool with_head_control) { robotis_controller_msgs::JointCtrlModule control_msg; std::string head_module = "head_control_module"; std::map::iterator joint_iter; - for (joint_iter = id_joint_table_.begin(); joint_iter != id_joint_table_.end(); ++joint_iter) - { + for (joint_iter = id_joint_table_.begin(); + joint_iter != id_joint_table_.end(); ++joint_iter) { // check whether joint name contains "head" - if (joint_iter->second.find("head") != std::string::npos) - { - if (with_head_control == true) - { + if (joint_iter->second.find("head") != std::string::npos) { + if (with_head_control == true) { control_msg.joint_name.push_back(joint_iter->second); control_msg.module_name.push_back(head_module); - } - else + } else continue; - } - else - { + } else { control_msg.joint_name.push_back(joint_iter->second); control_msg.module_name.push_back(body_module); } @@ -293,16 +265,15 @@ void SoccerDemo::setBodyModuleToDemo(const std::string &body_module, bool with_h std::cout << "enable module of body : " << body_module << std::endl; } -void SoccerDemo::setModuleToDemo(const std::string &module_name) -{ - if(enable_ == false) +void SoccerDemo::setModuleToDemo(const std::string &module_name) { + if (enable_ == false) return; robotis_controller_msgs::JointCtrlModule control_msg; std::map::iterator joint_iter; - for (joint_iter = id_joint_table_.begin(); joint_iter != id_joint_table_.end(); ++joint_iter) - { + for (joint_iter = id_joint_table_.begin(); + joint_iter != id_joint_table_.end(); ++joint_iter) { control_msg.joint_name.push_back(joint_iter->second); control_msg.module_name.push_back(module_name); } @@ -315,38 +286,34 @@ void SoccerDemo::setModuleToDemo(const std::string &module_name) std::cout << "enable module : " << module_name << std::endl; } -void SoccerDemo::callServiceSettingModule(const robotis_controller_msgs::JointCtrlModule &modules) -{ +void SoccerDemo::callServiceSettingModule( + const robotis_controller_msgs::JointCtrlModule &modules) { robotis_controller_msgs::SetJointModule set_joint_srv; set_joint_srv.request.joint_name = modules.joint_name; set_joint_srv.request.module_name = modules.module_name; - if (set_joint_module_client_.call(set_joint_srv) == false) - { + if (set_joint_module_client_.call(set_joint_srv) == false) { ROS_ERROR("Failed to set module"); return; } - return ; + return; } -void SoccerDemo::parseJointNameFromYaml(const std::string &path) -{ +void SoccerDemo::parseJointNameFromYaml(const std::string &path) { YAML::Node doc; - try - { + try { // load yaml doc = YAML::LoadFile(path.c_str()); - } catch (const std::exception& e) - { + } catch (const std::exception &e) { ROS_ERROR("Fail to load id_joint table yaml."); return; } // parse id_joint table YAML::Node _id_sub_node = doc["id_joint"]; - for (YAML::iterator _it = _id_sub_node.begin(); _it != _id_sub_node.end(); ++_it) - { + for (YAML::iterator _it = _id_sub_node.begin(); _it != _id_sub_node.end(); + ++_it) { int _id; std::string _joint_name; @@ -359,8 +326,7 @@ void SoccerDemo::parseJointNameFromYaml(const std::string &path) } // joint id -> joint name -bool SoccerDemo::getJointNameFromID(const int &id, std::string &joint_name) -{ +bool SoccerDemo::getJointNameFromID(const int &id, std::string &joint_name) { std::map::iterator _iter; _iter = id_joint_table_.find(id); @@ -372,8 +338,7 @@ bool SoccerDemo::getJointNameFromID(const int &id, std::string &joint_name) } // joint name -> joint id -bool SoccerDemo::getIDFromJointName(const std::string &joint_name, int &id) -{ +bool SoccerDemo::getIDFromJointName(const std::string &joint_name, int &id) { std::map::iterator _iter; _iter = joint_id_table_.find(joint_name); @@ -384,17 +349,13 @@ bool SoccerDemo::getIDFromJointName(const std::string &joint_name, int &id) return true; } -int SoccerDemo::getJointCount() -{ - return joint_id_table_.size(); -} +int SoccerDemo::getJointCount() { return joint_id_table_.size(); } -bool SoccerDemo::isHeadJoint(const int &id) -{ +bool SoccerDemo::isHeadJoint(const int &id) { std::map::iterator _iter; - for (_iter = joint_id_table_.begin(); _iter != joint_id_table_.end(); ++_iter) - { + for (_iter = joint_id_table_.begin(); _iter != joint_id_table_.end(); + ++_iter) { if (_iter->first.find("head") != std::string::npos) return true; } @@ -402,13 +363,11 @@ bool SoccerDemo::isHeadJoint(const int &id) return false; } -void SoccerDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) -{ +void SoccerDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { if (enable_ == false) return; - if (msg->data == "start") - { + if (msg->data == "start") { if (on_following_ball_ == true) stopSoccerMode(); else @@ -416,38 +375,36 @@ void SoccerDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) } } -void SoccerDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) -{ +void SoccerDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) { if (enable_ == false) return; - if (msg->data == "start") - { + if (msg->data == "start") { if (on_following_ball_ == true) stopSoccerMode(); else startSoccerMode(); - } - else if (msg->data == "stop") - { + } else if (msg->data == "stop") { stopSoccerMode(); } } // check fallen states -void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) -{ +void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr &msg) { if (enable_ == false) return; if (stop_fallen_check_ == true) return; - Eigen::Quaterniond orientation(msg->orientation.w, msg->orientation.x, msg->orientation.y, msg->orientation.z); - Eigen::MatrixXd rpy_orientation = robotis_framework::convertQuaternionToRPY(orientation); + Eigen::Quaterniond orientation(msg->orientation.w, msg->orientation.x, + msg->orientation.y, msg->orientation.z); + Eigen::MatrixXd rpy_orientation = + robotis_framework::convertQuaternionToRPY(orientation); rpy_orientation *= (180 / M_PI); - ROS_INFO_COND(DEBUG_PRINT, "Roll : %3.2f, Pitch : %2.2f", rpy_orientation.coeff(0, 0), rpy_orientation.coeff(1, 0)); + ROS_INFO_COND(DEBUG_PRINT, "Roll : %3.2f, Pitch : %2.2f", + rpy_orientation.coeff(0, 0), rpy_orientation.coeff(1, 0)); double pitch = rpy_orientation.coeff(1, 0); @@ -465,8 +422,7 @@ void SoccerDemo::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) stand_state_ = Stand; } -void SoccerDemo::startSoccerMode() -{ +void SoccerDemo::startSoccerMode() { setModuleToDemo("action_module"); playMotion(WalkingReady); @@ -479,16 +435,14 @@ void SoccerDemo::startSoccerMode() start_following_ = true; } -void SoccerDemo::stopSoccerMode() -{ +void SoccerDemo::stopSoccerMode() { ROS_INFO("Stop Soccer Demo"); on_following_ball_ = false; on_tracking_ball_ = false; stop_following_ = true; } -void SoccerDemo::handleKick(int ball_position) -{ +void SoccerDemo::handleKick(int ball_position) { usleep(1500 * 1000); // change to motion module @@ -498,8 +452,7 @@ void SoccerDemo::handleKick(int ball_position) return; // kick motion - switch (ball_position) - { + switch (ball_position) { case robotis_op::BallFollower::OnRight: std::cout << "Kick Motion [R]: " << ball_position << std::endl; playMotion(is_grass_ ? RightKick + ForGrass : RightKick); @@ -525,11 +478,10 @@ void SoccerDemo::handleKick(int ball_position) return; // ceremony - //playMotion(Ceremony); + // playMotion(Ceremony); } -void SoccerDemo::handleKick() -{ +void SoccerDemo::handleKick() { usleep(1500 * 1000); // change to motion module @@ -539,10 +491,11 @@ void SoccerDemo::handleKick() return; // kick motion - ball_follower_.decideBallPositin(ball_tracker_.getPanOfBall(), ball_tracker_.getTiltOfBall()); + ball_follower_.decideBallPositin(ball_tracker_.getPanOfBall(), + ball_tracker_.getTiltOfBall()); int ball_position = ball_follower_.getBallPosition(); - if(ball_position == BallFollower::NotFound || ball_position == BallFollower::OutOfRange) - { + if (ball_position == BallFollower::NotFound || + ball_position == BallFollower::OutOfRange) { on_following_ball_ = false; restart_soccer_ = true; tracking_status_ = BallTracker::NotFound; @@ -550,8 +503,7 @@ void SoccerDemo::handleKick() return; } - switch (ball_position) - { + switch (ball_position) { case robotis_op::BallFollower::OnRight: std::cout << "Kick Motion [R]: " << ball_position << std::endl; sendDebugTopic("Kick the ball using Right foot"); @@ -579,13 +531,11 @@ void SoccerDemo::handleKick() return; // ceremony - //playMotion(Ceremony); + // playMotion(Ceremony); } -bool SoccerDemo::handleFallen(int fallen_status) -{ - if (fallen_status == Stand) - { +bool SoccerDemo::handleFallen(int fallen_status) { + if (fallen_status == Stand) { return false; } @@ -593,8 +543,7 @@ bool SoccerDemo::handleFallen(int fallen_status) setModuleToDemo("action_module"); // getup motion - switch (fallen_status) - { + switch (fallen_status) { case Fallen_Forward: std::cout << "Getup Motion [F]: " << std::endl; playMotion(is_grass_ ? GetUpFront + ForGrass : GetUpFront); @@ -609,7 +558,7 @@ bool SoccerDemo::handleFallen(int fallen_status) break; } - while(isActionRunning() == true) + while (isActionRunning() == true) usleep(100 * 1000); usleep(650 * 1000); @@ -623,18 +572,17 @@ bool SoccerDemo::handleFallen(int fallen_status) return true; } -void SoccerDemo::playMotion(int motion_index) -{ +void SoccerDemo::playMotion(int motion_index) { std_msgs::Int32 motion_msg; motion_msg.data = motion_index; motion_index_pub_.publish(motion_msg); } -void SoccerDemo::setRGBLED(int blue, int green, int red) -{ +void SoccerDemo::setRGBLED(int blue, int green, int red) { int led_full_unit = 0x1F; - int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | (red & led_full_unit); + int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | + (red & led_full_unit); robotis_controller_msgs::SyncWriteItem syncwrite_msg; syncwrite_msg.item_name = "LED_RGB"; syncwrite_msg.joint_name.push_back("open-cr"); @@ -644,19 +592,14 @@ void SoccerDemo::setRGBLED(int blue, int green, int red) } // check running of action -bool SoccerDemo::isActionRunning() -{ +bool SoccerDemo::isActionRunning() { op3_action_module_msgs::IsRunning is_running_srv; - if (is_running_client_.call(is_running_srv) == false) - { + if (is_running_client_.call(is_running_srv) == false) { ROS_ERROR("Failed to get action status"); return true; - } - else - { - if (is_running_srv.response.is_running == true) - { + } else { + if (is_running_srv.response.is_running == true) { return true; } } @@ -664,12 +607,11 @@ bool SoccerDemo::isActionRunning() return false; } -void SoccerDemo::sendDebugTopic(const std::string &msgs) -{ +void SoccerDemo::sendDebugTopic(const std::string &msgs) { std_msgs::String debug_msg; debug_msg.data = msgs; test_pub_.publish(debug_msg); } -} +} // namespace robotis_op diff --git a/op3_demo/src/test/button_test.cpp b/op3_demo/src/test/button_test.cpp index 7a78c6a..20049f3 100644 --- a/op3_demo/src/test/button_test.cpp +++ b/op3_demo/src/test/button_test.cpp @@ -1,90 +1,76 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_demo/button_test.h" -namespace robotis_op -{ +namespace robotis_op { -ButtonTest::ButtonTest() - : SPIN_RATE(30), - led_count_(0), - rgb_led_count_(0) -{ +ButtonTest::ButtonTest() : SPIN_RATE(30), led_count_(0), rgb_led_count_(0) { enable_ = false; ros::NodeHandle nh(ros::this_node::getName()); - boost::thread queue_thread = boost::thread(boost::bind(&ButtonTest::callbackThread, this)); - boost::thread process_thread = boost::thread(boost::bind(&ButtonTest::processThread, this)); + boost::thread queue_thread = + boost::thread(boost::bind(&ButtonTest::callbackThread, this)); + boost::thread process_thread = + boost::thread(boost::bind(&ButtonTest::processThread, this)); default_mp3_path_ = ros::package::getPath("op3_demo") + "/data/mp3/test/"; } -ButtonTest::~ButtonTest() -{ -} +ButtonTest::~ButtonTest() {} -void ButtonTest::setDemoEnable() -{ +void ButtonTest::setDemoEnable() { enable_ = true; ROS_INFO("Start Button Test"); } -void ButtonTest::setDemoDisable() -{ - enable_ = false; -} +void ButtonTest::setDemoDisable() { enable_ = false; } -void ButtonTest::process() -{ +void ButtonTest::process() {} -} - -void ButtonTest::processThread() -{ - //set node loop rate +void ButtonTest::processThread() { + // set node loop rate ros::Rate loop_rate(SPIN_RATE); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { if (enable_ == true) process(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } } -void ButtonTest::callbackThread() -{ +void ButtonTest::callbackThread() { ros::NodeHandle nh(ros::this_node::getName()); // subscriber & publisher - rgb_led_pub_ = nh.advertise("/robotis/sync_write_item", 0); + rgb_led_pub_ = nh.advertise( + "/robotis/sync_write_item", 0); play_sound_pub_ = nh.advertise("/play_sound_file", 0); - buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &ButtonTest::buttonHandlerCallback, this); + buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, + &ButtonTest::buttonHandlerCallback, this); - while (nh.ok()) - { + while (nh.ok()) { ros::spinOnce(); usleep(1 * 1000); @@ -92,48 +78,37 @@ void ButtonTest::callbackThread() } // button test -void ButtonTest::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) -{ +void ButtonTest::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { if (enable_ == false) return; - if (msg->data == "mode") - { + if (msg->data == "mode") { playSound(default_mp3_path_ + "Mode button pressed.mp3"); - } - else if (msg->data == "start") - { + } else if (msg->data == "start") { // RGB led color test playSound(default_mp3_path_ + "Start button pressed.mp3"); - int rgb_selector[3] = { 1, 0, 0 }; - setRGBLED((0x1F * rgb_selector[rgb_led_count_ % 3]), (0x1F * rgb_selector[(rgb_led_count_ + 1) % 3]), + int rgb_selector[3] = {1, 0, 0}; + setRGBLED((0x1F * rgb_selector[rgb_led_count_ % 3]), + (0x1F * rgb_selector[(rgb_led_count_ + 1) % 3]), (0x1F * rgb_selector[(rgb_led_count_ + 2) % 3])); rgb_led_count_ += 1; - } - else if (msg->data == "user") - { + } else if (msg->data == "user") { // Monochromatic led color test playSound(default_mp3_path_ + "User button pressed.mp3"); setLED(0x01 << (led_count_++ % 3)); - } - else if (msg->data == "mode_long") - { + } else if (msg->data == "mode_long") { playSound(default_mp3_path_ + "Mode button long pressed.mp3"); - } - else if (msg->data == "start_long") - { + } else if (msg->data == "start_long") { playSound(default_mp3_path_ + "Start button long pressed.mp3"); - } - else if (msg->data == "user_long") - { + } else if (msg->data == "user_long") { playSound(default_mp3_path_ + "User button long pressed.mp3"); } } -void ButtonTest::setRGBLED(int blue, int green, int red) -{ +void ButtonTest::setRGBLED(int blue, int green, int red) { int led_full_unit = 0x1F; - int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | (red & led_full_unit); + int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | + (red & led_full_unit); robotis_controller_msgs::SyncWriteItem syncwrite_msg; syncwrite_msg.item_name = "LED_RGB"; syncwrite_msg.joint_name.push_back("open-cr"); @@ -142,8 +117,7 @@ void ButtonTest::setRGBLED(int blue, int green, int red) rgb_led_pub_.publish(syncwrite_msg); } -void ButtonTest::setLED(int led) -{ +void ButtonTest::setLED(int led) { robotis_controller_msgs::SyncWriteItem syncwrite_msg; syncwrite_msg.item_name = "LED"; syncwrite_msg.joint_name.push_back("open-cr"); @@ -152,8 +126,7 @@ void ButtonTest::setLED(int led) rgb_led_pub_.publish(syncwrite_msg); } -void ButtonTest::playSound(const std::string &path) -{ +void ButtonTest::playSound(const std::string &path) { std_msgs::String sound_msg; sound_msg.data = path; diff --git a/op3_demo/src/test/mic_test.cpp b/op3_demo/src/test/mic_test.cpp index 4e56954..61bc1ea 100644 --- a/op3_demo/src/test/mic_test.cpp +++ b/op3_demo/src/test/mic_test.cpp @@ -1,53 +1,47 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_demo/mic_test.h" -namespace robotis_op -{ +namespace robotis_op { MicTest::MicTest() - : SPIN_RATE(30), - is_wait_(false), - wait_time_(-1), - test_status_(Ready), - record_pid_(-1), - play_pid_(-1) -{ + : SPIN_RATE(30), is_wait_(false), wait_time_(-1), test_status_(Ready), + record_pid_(-1), play_pid_(-1) { enable_ = false; ros::NodeHandle nh(ros::this_node::getName()); - boost::thread queue_thread = boost::thread(boost::bind(&MicTest::callbackThread, this)); - boost::thread process_thread = boost::thread(boost::bind(&MicTest::processThread, this)); + boost::thread queue_thread = + boost::thread(boost::bind(&MicTest::callbackThread, this)); + boost::thread process_thread = + boost::thread(boost::bind(&MicTest::processThread, this)); - recording_file_name_ = ros::package::getPath("op3_demo") + "/data/mp3/test/mic-test.wav"; + recording_file_name_ = + ros::package::getPath("op3_demo") + "/data/mp3/test/mic-test.wav"; default_mp3_path_ = ros::package::getPath("op3_demo") + "/data/mp3/test/"; start_time_ = ros::Time::now(); } -MicTest::~MicTest() -{ -} +MicTest::~MicTest() {} -void MicTest::setDemoEnable() -{ +void MicTest::setDemoEnable() { wait_time_ = -1; test_status_ = AnnounceRecording; enable_ = true; @@ -55,125 +49,108 @@ void MicTest::setDemoEnable() ROS_INFO("Start Mic test Demo"); } -void MicTest::setDemoDisable() -{ +void MicTest::setDemoDisable() { finishTimer(); test_status_ = Ready; enable_ = false; } -void MicTest::process() -{ +void MicTest::process() { // check status // timer - if (wait_time_ > 0) - { + if (wait_time_ > 0) { ros::Duration dur = ros::Time::now() - start_time_; // check timer - if (dur.sec >= wait_time_) - { + if (dur.sec >= wait_time_) { finishTimer(); } - } - else if (wait_time_ == -1.0) - { + } else if (wait_time_ == -1.0) { // handle test process - switch (test_status_) - { - case Ready: - // do nothing - break; + switch (test_status_) { + case Ready: + // do nothing + break; - case AnnounceRecording: - announceTest(); - test_status_ = MicRecording; - break; + case AnnounceRecording: + announceTest(); + test_status_ = MicRecording; + break; - case MicRecording: - recordSound(); - test_status_ = PlayingSound; - break; + case MicRecording: + recordSound(); + test_status_ = PlayingSound; + break; - case PlayingSound: - playTestSound(recording_file_name_); - test_status_ = DeleteTempFile; - break; + case PlayingSound: + playTestSound(recording_file_name_); + test_status_ = DeleteTempFile; + break; - case DeleteTempFile: - deleteSoundFile(recording_file_name_); - test_status_ = Ready; - break; + case DeleteTempFile: + deleteSoundFile(recording_file_name_); + test_status_ = Ready; + break; - default: - break; + default: + break; } } - } -void MicTest::processThread() -{ - //set node loop rate +void MicTest::processThread() { + // set node loop rate ros::Rate loop_rate(SPIN_RATE); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { if (enable_ == true) process(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } } -void MicTest::callbackThread() -{ +void MicTest::callbackThread() { ros::NodeHandle nh(ros::this_node::getName()); // subscriber & publisher - buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &MicTest::buttonHandlerCallback, this); + buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, + &MicTest::buttonHandlerCallback, this); play_sound_pub_ = nh.advertise("/play_sound_file", 0); - while (nh.ok()) - { + while (nh.ok()) { ros::spinOnce(); usleep(1 * 1000); } } -void MicTest::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) -{ +void MicTest::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { if (enable_ == false) return; - if (msg->data == "start") - { + if (msg->data == "start") { // restart mic test if (test_status_ != Ready) return; test_status_ = AnnounceRecording; - } - else if(msg->data == "user") - { + } else if (msg->data == "user") { is_wait_ = true; } } -void MicTest::announceTest() -{ +void MicTest::announceTest() { // play mic test sound playSound(default_mp3_path_ + "Announce mic test.mp3"); usleep(3.4 * 1000 * 1000); } -void MicTest::recordSound(int recording_time) -{ +void MicTest::recordSound(int recording_time) { ROS_INFO("Start to record"); playSound(default_mp3_path_ + "Start recording.mp3"); @@ -185,36 +162,31 @@ void MicTest::recordSound(int recording_time) record_pid_ = fork(); - switch (record_pid_) - { - case -1: - fprintf(stderr, "Fork Failed!! \n"); - ROS_WARN("Fork Failed!! \n"); - break; + switch (record_pid_) { + case -1: + fprintf(stderr, "Fork Failed!! \n"); + ROS_WARN("Fork Failed!! \n"); + break; - case 0: - { - std::stringstream ss; - ss << "-d " << recording_time; - execl("/usr/bin/arecord", "arecord", "-Dplughw:1,0", "-fS16_LE", "-c1", "-r22050", "-twav", ss.str().c_str(), - recording_file_name_.c_str(), (char*) 0); - break; - } + case 0: { + std::stringstream ss; + ss << "-d " << recording_time; + execl("/usr/bin/arecord", "arecord", "-Dplughw:1,0", "-fS16_LE", "-c1", + "-r22050", "-twav", ss.str().c_str(), recording_file_name_.c_str(), + (char *)0); + break; + } - default: - break; + default: + break; } startTimer(recording_time); } -void MicTest::recordSound() -{ - recordSound(5); -} +void MicTest::recordSound() { recordSound(5); } -void MicTest::playTestSound(const std::string &path) -{ +void MicTest::playTestSound(const std::string &path) { ROS_INFO("Start to play recording sound"); playSound(default_mp3_path_ + "Start playing.mp3"); @@ -226,47 +198,40 @@ void MicTest::playTestSound(const std::string &path) play_pid_ = fork(); - switch (play_pid_) - { - case -1: - fprintf(stderr, "Fork Failed!! \n"); - ROS_WARN("Fork Failed!! \n"); - break; + switch (play_pid_) { + case -1: + fprintf(stderr, "Fork Failed!! \n"); + ROS_WARN("Fork Failed!! \n"); + break; - case 0: - execl("/usr/bin/aplay", "aplay", path.c_str(), (char*) 0); - break; + case 0: + execl("/usr/bin/aplay", "aplay", path.c_str(), (char *)0); + break; - default: - break; + default: + break; } startTimer(5); } -void MicTest::playSound(const std::string &path) -{ +void MicTest::playSound(const std::string &path) { std_msgs::String sound_msg; sound_msg.data = path; play_sound_pub_.publish(sound_msg); } -void MicTest::deleteSoundFile(const std::string &file_path) -{ +void MicTest::deleteSoundFile(const std::string &file_path) { remove(file_path.c_str()); ROS_INFO("Delete temporary file"); } -void MicTest::startTimer(double wait_time) -{ +void MicTest::startTimer(double wait_time) { start_time_ = ros::Time::now(); wait_time_ = wait_time; } -void MicTest::finishTimer() -{ - wait_time_ = -1; -} +void MicTest::finishTimer() { wait_time_ = -1; } } /* namespace robotis_op */ diff --git a/op3_demo/src/test_node.cpp b/op3_demo/src/test_node.cpp index 8d4768d..65e85ea 100644 --- a/op3_demo/src/test_node.cpp +++ b/op3_demo/src/test_node.cpp @@ -1,35 +1,34 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ -#include #include +#include #include -#include "op3_demo/soccer_demo.h" #include "op3_demo/action_demo.h" -#include "op3_demo/vision_demo.h" #include "op3_demo/button_test.h" #include "op3_demo/mic_test.h" -#include "robotis_math/robotis_linear_algebra.h" +#include "op3_demo/soccer_demo.h" +#include "op3_demo/vision_demo.h" #include "robotis_controller_msgs/SyncWriteItem.h" +#include "robotis_math/robotis_linear_algebra.h" -enum Demo_Status -{ +enum Demo_Status { Ready = 0, ButtonTest = 1, MicTest = 2, @@ -39,11 +38,11 @@ enum Demo_Status DemoCount = 6, }; -void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); +void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); void goInitPose(); void playSound(const std::string &path); void setLED(int led); -bool checkManagerRunning(std::string& manager_name); +bool checkManagerRunning(std::string &manager_name); void dxlTorqueChecker(); void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg); @@ -61,13 +60,12 @@ int current_status = Ready; int desired_status = Ready; bool apply_desired = false; -//node main -int main(int argc, char **argv) -{ - //init ros +// node main +int main(int argc, char **argv) { + // init ros ros::init(argc, argv, "self_test_node"); - //create ros wrapper object + // create ros wrapper object robotis_op::OPDemo *current_demo = NULL; robotis_op::SoccerDemo *soccer_demo = new robotis_op::SoccerDemo(); robotis_op::ActionDemo *action_demo = new robotis_op::ActionDemo(); @@ -79,26 +77,27 @@ int main(int argc, char **argv) init_pose_pub = nh.advertise("/robotis/base/ini_pose", 0); play_sound_pub = nh.advertise("/play_sound_file", 0); - led_pub = nh.advertise("/robotis/sync_write_item", 0); + led_pub = nh.advertise( + "/robotis/sync_write_item", 0); dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); - ros::Subscriber buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); - ros::Subscriber mode_command_sub = nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback); + ros::Subscriber buttuon_sub = + nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); + ros::Subscriber mode_command_sub = + nh.subscribe("/robotis/mode_command", 1, demoModeCommandCallback); default_mp3_path = ros::package::getPath("op3_demo") + "/data/mp3/"; ros::start(); - //set node loop rate + // set node loop rate ros::Rate loop_rate(SPIN_RATE); // wait for starting of manager std::string manager_name = "/op3_manager"; - while (ros::ok()) - { + while (ros::ok()) { ros::Duration(1.0).sleep(); - if (checkManagerRunning(manager_name) == true) - { + if (checkManagerRunning(manager_name) == true) { break; ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect"); } @@ -109,96 +108,85 @@ int main(int argc, char **argv) playSound(default_mp3_path + "test/Self test ready mode.mp3"); setLED(0x01 | 0x02 | 0x04); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { // process - if (apply_desired == true) - { - switch (desired_status) - { - case Ready: - { + if (apply_desired == true) { + switch (desired_status) { + case Ready: { - if (current_demo != NULL) - current_demo->setDemoDisable(); + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = NULL; + current_demo = NULL; - goInitPose(); + goInitPose(); - ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]"); - break; - } + ROS_INFO_COND(DEBUG_PRINT, "[Go to Demo READY!]"); + break; + } - case SoccerDemo: - { - if (current_demo != NULL) - current_demo->setDemoDisable(); + case SoccerDemo: { + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = soccer_demo; - current_demo->setDemoEnable(); + current_demo = soccer_demo; + current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo"); - break; - } + ROS_INFO_COND(DEBUG_PRINT, "[Start] Soccer Demo"); + break; + } - case VisionDemo: - { - if (current_demo != NULL) - current_demo->setDemoDisable(); + case VisionDemo: { + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = vision_demo; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo"); - break; - } - case ActionDemo: - { - if (current_demo != NULL) - current_demo->setDemoDisable(); + current_demo = vision_demo; + current_demo->setDemoEnable(); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Vision Demo"); + break; + } + case ActionDemo: { + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = action_demo; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo"); - break; - } - case ButtonTest: - { - if (current_demo != NULL) - current_demo->setDemoDisable(); + current_demo = action_demo; + current_demo->setDemoEnable(); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Action Demo"); + break; + } + case ButtonTest: { + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = button_test; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Button Test"); - break; - } - case MicTest: - { - if (current_demo != NULL) - current_demo->setDemoDisable(); + current_demo = button_test; + current_demo->setDemoEnable(); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Button Test"); + break; + } + case MicTest: { + if (current_demo != NULL) + current_demo->setDemoDisable(); - current_demo = mic_test; - current_demo->setDemoEnable(); - ROS_INFO_COND(DEBUG_PRINT, "[Start] Mic Test"); - break; - } + current_demo = mic_test; + current_demo->setDemoEnable(); + ROS_INFO_COND(DEBUG_PRINT, "[Start] Mic Test"); + break; + } - - default: - { - break; - } + default: { + break; + } } apply_desired = false; current_status = desired_status; } - //execute pending callbacks + // execute pending callbacks ros::spinOnce(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); // for debug @@ -206,112 +194,104 @@ int main(int argc, char **argv) return 0; } - //exit program + // exit program return 0; } -void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) -{ - if(apply_desired == true) +void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { + if (apply_desired == true) return; // in the middle of playing demo - if (current_status != Ready) - { - if (msg->data == "mode_long") - { + if (current_status != Ready) { + if (msg->data == "mode_long") { // go to mode selection status desired_status = Ready; apply_desired = true; playSound(default_mp3_path + "test/Self test ready mode.mp3"); setLED(0x01 | 0x02 | 0x04); - } - else if (msg->data == "user_long") - { + } else if (msg->data == "user_long") { // it's using in op3_manager // torque on and going to init pose } } // ready to start demo - else - { - if (msg->data == "start") - { + else { + if (msg->data == "start") { // select current demo - desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status; + desired_status = + (desired_status == Ready) ? desired_status + 1 : desired_status; apply_desired = true; // sound out desired status - switch (desired_status) - { - case SoccerDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start soccer demonstration.mp3"); - break; + switch (desired_status) { + case SoccerDemo: + dxlTorqueChecker(); + playSound(default_mp3_path + "Start soccer demonstration.mp3"); + break; - case VisionDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start vision processing demonstration.mp3"); - break; + case VisionDemo: + dxlTorqueChecker(); + playSound(default_mp3_path + + "Start vision processing demonstration.mp3"); + break; - case ActionDemo: - dxlTorqueChecker(); - playSound(default_mp3_path + "Start motion demonstration.mp3"); - break; + case ActionDemo: + dxlTorqueChecker(); + playSound(default_mp3_path + "Start motion demonstration.mp3"); + break; - case ButtonTest: - dxlTorqueChecker(); - playSound(default_mp3_path + "test/Start button test mode.mp3"); - break; + case ButtonTest: + dxlTorqueChecker(); + playSound(default_mp3_path + "test/Start button test mode.mp3"); + break; - case MicTest: - dxlTorqueChecker(); - playSound(default_mp3_path + "test/Start mic test mode.mp3"); - break; + case MicTest: + dxlTorqueChecker(); + playSound(default_mp3_path + "test/Start mic test mode.mp3"); + break; - default: - break; + default: + break; } ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } - else if (msg->data == "mode") - { + } else if (msg->data == "mode") { // change to next demo desired_status = (desired_status + 1) % DemoCount; - desired_status = (desired_status == Ready) ? desired_status + 1 : desired_status; + desired_status = + (desired_status == Ready) ? desired_status + 1 : desired_status; // sound out desired status and changign LED - switch (desired_status) - { - case SoccerDemo: - playSound(default_mp3_path + "Autonomous soccer mode.mp3"); - setLED(0x01); - break; + switch (desired_status) { + case SoccerDemo: + playSound(default_mp3_path + "Autonomous soccer mode.mp3"); + setLED(0x01); + break; - case VisionDemo: - playSound(default_mp3_path + "Vision processing mode.mp3"); - setLED(0x02); - break; + case VisionDemo: + playSound(default_mp3_path + "Vision processing mode.mp3"); + setLED(0x02); + break; - case ActionDemo: - playSound(default_mp3_path + "Interactive motion mode.mp3"); - setLED(0x04); - break; + case ActionDemo: + playSound(default_mp3_path + "Interactive motion mode.mp3"); + setLED(0x04); + break; - case ButtonTest: - playSound(default_mp3_path + "test/Button test mode.mp3"); - setLED(0x01 | 0x02); - break; + case ButtonTest: + playSound(default_mp3_path + "test/Button test mode.mp3"); + setLED(0x01 | 0x02); + break; - case MicTest: - playSound(default_mp3_path + "test/Mic test mode.mp3"); - setLED(0x01 | 0x04); - break; + case MicTest: + playSound(default_mp3_path + "test/Mic test mode.mp3"); + setLED(0x01 | 0x04); + break; - default: - break; + default: + break; } ROS_INFO_COND(DEBUG_PRINT, "= Demo Mode : %d", desired_status); @@ -319,24 +299,21 @@ void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) } } -void goInitPose() -{ +void goInitPose() { std_msgs::String init_msg; init_msg.data = "ini_pose"; init_pose_pub.publish(init_msg); } -void playSound(const std::string &path) -{ +void playSound(const std::string &path) { std_msgs::String sound_msg; sound_msg.data = path; play_sound_pub.publish(sound_msg); } -void setLED(int led) -{ +void setLED(int led) { robotis_controller_msgs::SyncWriteItem syncwrite_msg; syncwrite_msg.item_name = "LED"; syncwrite_msg.joint_name.push_back("open-cr"); @@ -345,13 +322,12 @@ void setLED(int led) led_pub.publish(syncwrite_msg); } -bool checkManagerRunning(std::string& manager_name) -{ +bool checkManagerRunning(std::string &manager_name) { std::vector node_list; ros::master::getNodes(node_list); - for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); node_list_idx++) - { + for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); + node_list_idx++) { if (node_list[node_list_idx] == manager_name) return true; } @@ -360,21 +336,17 @@ bool checkManagerRunning(std::string& manager_name) return false; } -void dxlTorqueChecker() -{ +void dxlTorqueChecker() { std_msgs::String check_msg; check_msg.data = "check"; dxl_torque_pub.publish(check_msg); } -void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) -{ +void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) { // In demo mode - if (current_status != Ready) - { - if (msg->data == "ready") - { + if (current_status != Ready) { + if (msg->data == "ready") { // go to mode selection status desired_status = Ready; apply_desired = true; @@ -384,10 +356,8 @@ void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) } } // In ready mode - else - { - if(msg->data == "soccer") - { + else { + if (msg->data == "soccer") { desired_status = SoccerDemo; apply_desired = true; @@ -395,9 +365,7 @@ void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) dxlTorqueChecker(); playSound(default_mp3_path + "Start soccer demonstration.mp3"); ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } - else if(msg->data == "vision") - { + } else if (msg->data == "vision") { desired_status = VisionDemo; apply_desired = true; @@ -405,9 +373,7 @@ void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) dxlTorqueChecker(); playSound(default_mp3_path + "Start vision processing demonstration.mp3"); ROS_INFO_COND(DEBUG_PRINT, "= Start Demo Mode : %d", desired_status); - } - else if(msg->data == "action") - { + } else if (msg->data == "action") { desired_status = ActionDemo; apply_desired = true; @@ -418,4 +384,3 @@ void demoModeCommandCallback(const std_msgs::String::ConstPtr &msg) } } } - diff --git a/op3_demo/src/vision/face_tracker.cpp b/op3_demo/src/vision/face_tracker.cpp index 771607f..a0079f2 100644 --- a/op3_demo/src/vision/face_tracker.cpp +++ b/op3_demo/src/vision/face_tracker.cpp @@ -1,68 +1,59 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_demo/face_tracker.h" -namespace robotis_op -{ +namespace robotis_op { FaceTracker::FaceTracker() - : nh_(ros::this_node::getName()), - FOV_WIDTH(35.2 * M_PI / 180), - FOV_HEIGHT(21.6 * M_PI / 180), - NOT_FOUND_THRESHOLD(50), - use_head_scan_(false), - count_not_found_(0), - on_tracking_(false) -{ - head_joint_offset_pub_ = nh_.advertise("/robotis/head_control/set_joint_states_offset", 0); - head_joint_pub_ = nh_.advertise("/robotis/head_control/set_joint_states", 0); - head_scan_pub_ = nh_.advertise("/robotis/head_control/scan_command", 0); + : nh_(ros::this_node::getName()), FOV_WIDTH(35.2 * M_PI / 180), + FOV_HEIGHT(21.6 * M_PI / 180), NOT_FOUND_THRESHOLD(50), + use_head_scan_(false), count_not_found_(0), on_tracking_(false) { + head_joint_offset_pub_ = nh_.advertise( + "/robotis/head_control/set_joint_states_offset", 0); + head_joint_pub_ = nh_.advertise( + "/robotis/head_control/set_joint_states", 0); + head_scan_pub_ = + nh_.advertise("/robotis/head_control/scan_command", 0); - face_position_sub_ = nh_.subscribe("/face_position", 1, &FaceTracker::facePositionCallback, this); - //face_tracking_command_sub_ = nh_.subscribe("/robotis/demo_command", 1, &FaceTracker::faceTrackerCommandCallback, this); + face_position_sub_ = nh_.subscribe("/face_position", 1, + &FaceTracker::facePositionCallback, this); + // face_tracking_command_sub_ = nh_.subscribe("/robotis/demo_command", 1, + // &FaceTracker::faceTrackerCommandCallback, this); } -FaceTracker::~FaceTracker() -{ +FaceTracker::~FaceTracker() {} -} - -void FaceTracker::facePositionCallback(const geometry_msgs::Point::ConstPtr &msg) -{ +void FaceTracker::facePositionCallback( + const geometry_msgs::Point::ConstPtr &msg) { if (msg->z < 0) return; face_position_ = *msg; } -void FaceTracker::faceTrackerCommandCallback(const std_msgs::String::ConstPtr &msg) -{ - if (msg->data == "start") - { +void FaceTracker::faceTrackerCommandCallback( + const std_msgs::String::ConstPtr &msg) { + if (msg->data == "start") { startTracking(); - } - else if (msg->data == "stop") - { + } else if (msg->data == "stop") { stopTracking(); - } - else if (msg->data == "toggle_start") - { + } else if (msg->data == "toggle_start") { if (on_tracking_ == false) startTracking(); else @@ -70,35 +61,27 @@ void FaceTracker::faceTrackerCommandCallback(const std_msgs::String::ConstPtr &m } } -void FaceTracker::startTracking() -{ +void FaceTracker::startTracking() { on_tracking_ = true; ROS_INFO("Start Face tracking"); } -void FaceTracker::stopTracking() -{ +void FaceTracker::stopTracking() { on_tracking_ = false; ROS_INFO("Stop Face tracking"); } -void FaceTracker::setUsingHeadScan(bool use_scan) -{ - use_head_scan_ = use_scan; -} +void FaceTracker::setUsingHeadScan(bool use_scan) { use_head_scan_ = use_scan; } -void FaceTracker::setFacePosition(geometry_msgs::Point &face_position) -{ - if (face_position.z > 0) - { +void FaceTracker::setFacePosition(geometry_msgs::Point &face_position) { + if (face_position.z > 0) { face_position_ = face_position; } } -void FaceTracker::goInit(double init_pan, double init_tile) -{ +void FaceTracker::goInit(double init_pan, double init_tile) { sensor_msgs::JointState head_angle_msg; head_angle_msg.name.push_back("head_pan"); @@ -110,37 +93,29 @@ void FaceTracker::goInit(double init_pan, double init_tile) head_joint_pub_.publish(head_angle_msg); } -int FaceTracker::processTracking() -{ - if (on_tracking_ == false) - { +int FaceTracker::processTracking() { + if (on_tracking_ == false) { face_position_.z = 0; count_not_found_ = 0; - //return false; + // return false; return Waiting; } // check ball position - if (face_position_.z <= 0) - { + if (face_position_.z <= 0) { count_not_found_++; - if (count_not_found_ == NOT_FOUND_THRESHOLD) - { + if (count_not_found_ == NOT_FOUND_THRESHOLD) { scanFace(); - //count_not_found_ = 0; + // count_not_found_ = 0; return NotFound; - } - else if (count_not_found_ > NOT_FOUND_THRESHOLD) - { + } else if (count_not_found_ > NOT_FOUND_THRESHOLD) { return NotFound; - } - else - { + } else { return Waiting; } - //return false; + // return false; } // if face is detected @@ -166,15 +141,14 @@ int FaceTracker::processTracking() return Found; } -void FaceTracker::publishHeadJoint(double pan, double tilt) -{ +void FaceTracker::publishHeadJoint(double pan, double tilt) { double min_angle = 1 * M_PI / 180; - if (fabs(pan) < min_angle && fabs(tilt) < min_angle) - { + if (fabs(pan) < min_angle && fabs(tilt) < min_angle) { dismissed_count_ += 1; return; } - std::cout << "Target angle[" << dismissed_count_ << "] : " << pan << " | " << tilt << std::endl; + std::cout << "Target angle[" << dismissed_count_ << "] : " << pan << " | " + << tilt << std::endl; dismissed_count_ = 0; @@ -189,8 +163,7 @@ void FaceTracker::publishHeadJoint(double pan, double tilt) head_joint_offset_pub_.publish(head_angle_msg); } -void FaceTracker::scanFace() -{ +void FaceTracker::scanFace() { if (use_head_scan_ == false) return; @@ -205,4 +178,4 @@ void FaceTracker::scanFace() // ROS_INFO("Scan the ball"); } -} +} // namespace robotis_op diff --git a/op3_demo/src/vision/vision_demo.cpp b/op3_demo/src/vision/vision_demo.cpp index 0045a7d..cc18b50 100644 --- a/op3_demo/src/vision/vision_demo.cpp +++ b/op3_demo/src/vision/vision_demo.cpp @@ -1,46 +1,42 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include "op3_demo/vision_demo.h" -namespace robotis_op -{ +namespace robotis_op { VisionDemo::VisionDemo() - : SPIN_RATE(30), - TIME_TO_INIT(10), - tracking_status_(FaceTracker::Waiting) -{ + : SPIN_RATE(30), TIME_TO_INIT(10), tracking_status_(FaceTracker::Waiting) { enable_ = false; ros::NodeHandle nh(ros::this_node::getName()); - boost::thread queue_thread = boost::thread(boost::bind(&VisionDemo::callbackThread, this)); - boost::thread process_thread = boost::thread(boost::bind(&VisionDemo::processThread, this)); + boost::thread queue_thread = + boost::thread(boost::bind(&VisionDemo::callbackThread, this)); + boost::thread process_thread = + boost::thread(boost::bind(&VisionDemo::processThread, this)); } -VisionDemo::~VisionDemo() -{ +VisionDemo::~VisionDemo() { // TODO Auto-generated destructor stub } -void VisionDemo::setDemoEnable() -{ +void VisionDemo::setDemoEnable() { // set prev time for timer prev_time_ = ros::Time::now(); @@ -63,11 +59,9 @@ void VisionDemo::setDemoEnable() face_tracker_.startTracking(); ROS_INFO("Start Vision Demo"); - } -void VisionDemo::setDemoDisable() -{ +void VisionDemo::setDemoDisable() { face_tracker_.stopTracking(); tracking_status_ = FaceTracker::Waiting; @@ -78,30 +72,24 @@ void VisionDemo::setDemoDisable() face_tracking_command_pub_.publish(command); } -void VisionDemo::process() -{ +void VisionDemo::process() { int tracking_status = face_tracker_.processTracking(); - - - switch(tracking_status) - { + switch (tracking_status) { case FaceTracker::Found: - if(tracking_status_ != tracking_status) + if (tracking_status_ != tracking_status) setRGBLED(0x1F, 0x1F, 0x1F); prev_time_ = ros::Time::now(); break; - case FaceTracker::NotFound: - { - if(tracking_status_ != tracking_status) + case FaceTracker::NotFound: { + if (tracking_status_ != tracking_status) setRGBLED(0, 0, 0); ros::Time curr_time = ros::Time::now(); ros::Duration dur = curr_time - prev_time_; - if(dur.sec > TIME_TO_INIT) - { - face_tracker_.goInit(0,0); + if (dur.sec > TIME_TO_INIT) { + face_tracker_.goInit(0, 0); prev_time_ = curr_time; } break; @@ -110,117 +98,106 @@ void VisionDemo::process() break; } - - if(tracking_status != FaceTracker::Waiting) + if (tracking_status != FaceTracker::Waiting) tracking_status_ = tracking_status; } -void VisionDemo::processThread() -{ - //set node loop rate +void VisionDemo::processThread() { + // set node loop rate ros::Rate loop_rate(SPIN_RATE); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { if (enable_ == true) process(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } } -void VisionDemo::callbackThread() -{ +void VisionDemo::callbackThread() { ros::NodeHandle nh(ros::this_node::getName()); // subscriber & publisher - module_control_pub_ = nh.advertise("/robotis/enable_ctrl_module", 0); - motion_index_pub_ = nh.advertise("/robotis/action/page_num", 0); - rgb_led_pub_ = nh.advertise("/robotis/sync_write_item", 0); - face_tracking_command_pub_ = nh.advertise("/face_tracking/command", 0); + module_control_pub_ = + nh.advertise("/robotis/enable_ctrl_module", 0); + motion_index_pub_ = + nh.advertise("/robotis/action/page_num", 0); + rgb_led_pub_ = nh.advertise( + "/robotis/sync_write_item", 0); + face_tracking_command_pub_ = + nh.advertise("/face_tracking/command", 0); - buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, &VisionDemo::buttonHandlerCallback, this); - faceCoord_sub_ = nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this); + buttuon_sub_ = nh.subscribe("/robotis/open_cr/button", 1, + &VisionDemo::buttonHandlerCallback, this); + faceCoord_sub_ = + nh.subscribe("/faceCoord", 1, &VisionDemo::facePositionCallback, this); - set_joint_module_client_ = nh.serviceClient("/robotis/set_present_ctrl_modules"); + set_joint_module_client_ = + nh.serviceClient( + "/robotis/set_present_ctrl_modules"); - while (nh.ok()) - { + while (nh.ok()) { ros::spinOnce(); usleep(1 * 1000); } } -void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) -{ +void VisionDemo::buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { if (enable_ == false) return; - if (msg->data == "start") - { - - } - else if (msg->data == "mode") - { + if (msg->data == "start") { + } else if (msg->data == "mode") { } } -void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) -{ +void VisionDemo::demoCommandCallback(const std_msgs::String::ConstPtr &msg) { if (enable_ == false) return; - if (msg->data == "start") - { - - } - else if (msg->data == "stop") - { + if (msg->data == "start") { + } else if (msg->data == "stop") { } } -void VisionDemo::setModuleToDemo(const std::string &module_name) -{ +void VisionDemo::setModuleToDemo(const std::string &module_name) { callServiceSettingModule(module_name); ROS_INFO_STREAM("enable module : " << module_name); } -void VisionDemo::callServiceSettingModule(const std::string &module_name) -{ +void VisionDemo::callServiceSettingModule(const std::string &module_name) { robotis_controller_msgs::SetModule set_module_srv; set_module_srv.request.module_name = module_name; - if (set_joint_module_client_.call(set_module_srv) == false) - { + if (set_joint_module_client_.call(set_module_srv) == false) { ROS_ERROR("Failed to set module"); return; } - return ; + return; } -void VisionDemo::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr &msg) -{ +void VisionDemo::facePositionCallback( + const std_msgs::Int32MultiArray::ConstPtr &msg) { if (enable_ == false) return; // face is detected - if (msg->data.size() >= 10) - { + if (msg->data.size() >= 10) { // center of face - face_position_.x = (msg->data[6] + msg->data[8] * 0.5) / msg->data[2] * 2 - 1; - face_position_.y = (msg->data[7] + msg->data[9] * 0.5) / msg->data[3] * 2 - 1; + face_position_.x = + (msg->data[6] + msg->data[8] * 0.5) / msg->data[2] * 2 - 1; + face_position_.y = + (msg->data[7] + msg->data[9] * 0.5) / msg->data[3] * 2 - 1; face_position_.z = msg->data[8] * 0.5 + msg->data[9] * 0.5; face_tracker_.setFacePosition(face_position_); - } - else - { + } else { face_position_.x = 0; face_position_.y = 0; face_position_.z = 0; @@ -228,18 +205,17 @@ void VisionDemo::facePositionCallback(const std_msgs::Int32MultiArray::ConstPtr } } -void VisionDemo::playMotion(int motion_index) -{ +void VisionDemo::playMotion(int motion_index) { std_msgs::Int32 motion_msg; motion_msg.data = motion_index; motion_index_pub_.publish(motion_msg); } -void VisionDemo::setRGBLED(int blue, int green, int red) -{ +void VisionDemo::setRGBLED(int blue, int green, int red) { int led_full_unit = 0x1F; - int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | (red & led_full_unit); + int led_value = (blue & led_full_unit) << 10 | (green & led_full_unit) << 5 | + (red & led_full_unit); robotis_controller_msgs::SyncWriteItem syncwrite_msg; syncwrite_msg.item_name = "LED_RGB"; syncwrite_msg.joint_name.push_back("open-cr"); diff --git a/op3_read_write_demo/CHANGELOG.rst b/op3_read_write_demo/CHANGELOG.rst new file mode 100644 index 0000000..6f0e796 --- /dev/null +++ b/op3_read_write_demo/CHANGELOG.rst @@ -0,0 +1,8 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotis_op3_demo +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.3.0 (2021-05-05) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande diff --git a/op3_read_write_demo/CMakeLists.txt b/op3_read_write_demo/CMakeLists.txt index 8826d03..3dfc844 100644 --- a/op3_read_write_demo/CMakeLists.txt +++ b/op3_read_write_demo/CMakeLists.txt @@ -1,13 +1,14 @@ ################################################################################ # Set minimum required version of cmake, project name and compile options ################################################################################ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(op3_read_write_demo) ################################################################################ # Find catkin packages and libraries for catkin and system dependencies ################################################################################ -find_package(catkin REQUIRED COMPONENTS +find_package( + catkin REQUIRED COMPONENTS robotis_controller_msgs roscpp sensor_msgs @@ -32,42 +33,45 @@ find_package(catkin REQUIRED COMPONENTS catkin_package( INCLUDE_DIRS CATKIN_DEPENDS - roscpp - robotis_controller_msgs - roscpp - sensor_msgs - std_msgs + roscpp + robotis_controller_msgs + roscpp + sensor_msgs + std_msgs ) ################################################################################ # Build ################################################################################ -include_directories( - ${catkin_INCLUDE_DIRS} -) +include_directories(${catkin_INCLUDE_DIRS}) -add_executable(read_write +add_executable( + read_write src/read_write.cpp ) -add_dependencies(read_write +add_dependencies( + read_write ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(read_write +target_link_libraries( + read_write ${catkin_LIBRARIES} ) ################################################################################ # Install ################################################################################ -install(TARGETS read_write +install( + TARGETS read_write RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) ################################################################################ diff --git a/op3_read_write_demo/launch/op3_read_write.launch b/op3_read_write_demo/launch/op3_read_write.launch index 01217ff..3c70adb 100644 --- a/op3_read_write_demo/launch/op3_read_write.launch +++ b/op3_read_write_demo/launch/op3_read_write.launch @@ -1,24 +1,24 @@ - - - - - - - - - - - - - + + + + + + + + + + + + + - - + + - + diff --git a/op3_read_write_demo/package.xml b/op3_read_write_demo/package.xml index 3d7c712..5cc8f71 100644 --- a/op3_read_write_demo/package.xml +++ b/op3_read_write_demo/package.xml @@ -1,23 +1,38 @@ op3_read_write_demo - 0.0.1 + 0.3.0 The op3_read_write_demo package - + Apache 2.0 Kayman - Pyo + Ronaldson Bellande + http://wiki.ros.org/op3_demo http://emanual.robotis.com/docs/en/platform/op3/introduction/ https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin - roscpp - std_msgs - sensor_msgs - robotis_controller_msgs + + roscpp + std_msgs + sensor_msgs + robotis_controller_msgs + op3_manager + + roscpp + std_msgs + sensor_msgs + robotis_controller_msgs + op3_manager + + roscpp + std_msgs + sensor_msgs + robotis_controller_msgs op3_manager diff --git a/op3_read_write_demo/src/read_write.cpp b/op3_read_write_demo/src/read_write.cpp index 1b87dc4..b415400 100644 --- a/op3_read_write_demo/src/read_write.cpp +++ b/op3_read_write_demo/src/read_write.cpp @@ -1,40 +1,39 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ /* Author: Kayman Jung */ #include -#include #include +#include #include "robotis_controller_msgs/SetModule.h" #include "robotis_controller_msgs/SyncWriteItem.h" -void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg); -void jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg); +void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg); +void jointstatesCallback(const sensor_msgs::JointState::ConstPtr &msg); void readyToDemo(); -void setModule(const std::string& module_name); +void setModule(const std::string &module_name); void goInitPose(); void setLED(int led); -bool checkManagerRunning(std::string& manager_name); +bool checkManagerRunning(std::string &manager_name); void torqueOnAll(); -void torqueOff(const std::string& body_side); +void torqueOff(const std::string &body_side); -enum ControlModule -{ +enum ControlModule { None = 0, DirectControlModule = 1, Framework = 2, @@ -56,39 +55,43 @@ ros::ServiceClient set_joint_module_client; int control_module = None; bool demo_ready = false; -//node main -int main(int argc, char **argv) -{ - //init ros +// node main +int main(int argc, char **argv) { + // init ros ros::init(argc, argv, "read_write"); ros::NodeHandle nh(ros::this_node::getName()); init_pose_pub = nh.advertise("/robotis/base/ini_pose", 0); - sync_write_pub = nh.advertise("/robotis/sync_write_item", 0); + sync_write_pub = nh.advertise( + "/robotis/sync_write_item", 0); dxl_torque_pub = nh.advertise("/robotis/dxl_torque", 0); - write_joint_pub = nh.advertise("/robotis/set_joint_states", 0); - write_joint_pub2 = nh.advertise("/robotis/direct_control/set_joint_states", 0); + write_joint_pub = + nh.advertise("/robotis/set_joint_states", 0); + write_joint_pub2 = nh.advertise( + "/robotis/direct_control/set_joint_states", 0); - read_joint_sub = nh.subscribe("/robotis/present_joint_states", 1, jointstatesCallback); - buttuon_sub = nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); + read_joint_sub = + nh.subscribe("/robotis/present_joint_states", 1, jointstatesCallback); + buttuon_sub = + nh.subscribe("/robotis/open_cr/button", 1, buttonHandlerCallback); // service - set_joint_module_client = nh.serviceClient("/robotis/set_present_ctrl_modules"); + set_joint_module_client = + nh.serviceClient( + "/robotis/set_present_ctrl_modules"); ros::start(); - //set node loop rate + // set node loop rate ros::Rate loop_rate(SPIN_RATE); // wait for starting of op3_manager std::string manager_name = "/op3_manager"; - while (ros::ok()) - { + while (ros::ok()) { ros::Duration(1.0).sleep(); - if (checkManagerRunning(manager_name) == true) - { + if (checkManagerRunning(manager_name) == true) { break; ROS_INFO_COND(DEBUG_PRINT, "Succeed to connect"); } @@ -97,76 +100,66 @@ int main(int argc, char **argv) readyToDemo(); - //node loop - while (ros::ok()) - { + // node loop + while (ros::ok()) { // process - //execute pending callbacks + // execute pending callbacks ros::spinOnce(); - //relax to fit output rate + // relax to fit output rate loop_rate.sleep(); } - //exit program + // exit program return 0; } -void buttonHandlerCallback(const std_msgs::String::ConstPtr& msg) -{ +void buttonHandlerCallback(const std_msgs::String::ConstPtr &msg) { // starting demo using robotis_controller - if (msg->data == "mode") - { + if (msg->data == "mode") { control_module = Framework; ROS_INFO("Button : mode | Framework"); readyToDemo(); } // starting demo using direct_control_module - else if (msg->data == "start") - { + else if (msg->data == "start") { control_module = DirectControlModule; ROS_INFO("Button : start | Direct control module"); readyToDemo(); } // torque on all joints of ROBOTIS-OP3 - else if (msg->data == "user") - { + else if (msg->data == "user") { torqueOnAll(); control_module = None; } } -void jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg) -{ - if(control_module == None) +void jointstatesCallback(const sensor_msgs::JointState::ConstPtr &msg) { + if (control_module == None) return; sensor_msgs::JointState write_msg; write_msg.header = msg->header; - for(int ix = 0; ix < msg->name.size(); ix++) - { + for (int ix = 0; ix < msg->name.size(); ix++) { std::string joint_name = msg->name[ix]; double joint_position = msg->position[ix]; // mirror and copy joint angles from right to left - if(joint_name == "r_sho_pitch") - { + if (joint_name == "r_sho_pitch") { write_msg.name.push_back("r_sho_pitch"); write_msg.position.push_back(joint_position); write_msg.name.push_back("l_sho_pitch"); write_msg.position.push_back(-joint_position); } - if(joint_name == "r_sho_roll") - { + if (joint_name == "r_sho_roll") { write_msg.name.push_back("r_sho_roll"); write_msg.position.push_back(joint_position); write_msg.name.push_back("l_sho_roll"); write_msg.position.push_back(-joint_position); } - if(joint_name == "r_el") - { + if (joint_name == "r_el") { write_msg.name.push_back("r_el"); write_msg.position.push_back(joint_position); write_msg.name.push_back("l_el"); @@ -175,14 +168,13 @@ void jointstatesCallback(const sensor_msgs::JointState::ConstPtr& msg) } // publish a message to set the joint angles - if(control_module == Framework) + if (control_module == Framework) write_joint_pub.publish(write_msg); - else if(control_module == DirectControlModule) + else if (control_module == DirectControlModule) write_joint_pub2.publish(write_msg); } -void readyToDemo() -{ +void readyToDemo() { ROS_INFO("Start Read-Write Demo"); // turn off LED setLED(0x04); @@ -201,17 +193,13 @@ void readyToDemo() setLED(control_module); // change the module for demo - if(control_module == Framework) - { + if (control_module == Framework) { setModule("none"); ROS_INFO("Change module to none"); - } - else if(control_module == DirectControlModule) - { + } else if (control_module == DirectControlModule) { setModule("direct_control_module"); ROS_INFO("Change module to direct_control_module"); - } - else + } else return; // torque off : right arm @@ -219,16 +207,14 @@ void readyToDemo() ROS_INFO("Torque off"); } -void goInitPose() -{ +void goInitPose() { std_msgs::String init_msg; init_msg.data = "ini_pose"; init_pose_pub.publish(init_msg); } -void setLED(int led) -{ +void setLED(int led) { robotis_controller_msgs::SyncWriteItem syncwrite_msg; syncwrite_msg.item_name = "LED"; syncwrite_msg.joint_name.push_back("open-cr"); @@ -237,13 +223,12 @@ void setLED(int led) sync_write_pub.publish(syncwrite_msg); } -bool checkManagerRunning(std::string& manager_name) -{ +bool checkManagerRunning(std::string &manager_name) { std::vector node_list; ros::master::getNodes(node_list); - for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); node_list_idx++) - { + for (unsigned int node_list_idx = 0; node_list_idx < node_list.size(); + node_list_idx++) { if (node_list[node_list_idx] == manager_name) return true; } @@ -252,53 +237,45 @@ bool checkManagerRunning(std::string& manager_name) return false; } -void setModule(const std::string& module_name) -{ +void setModule(const std::string &module_name) { robotis_controller_msgs::SetModule set_module_srv; set_module_srv.request.module_name = module_name; - if (set_joint_module_client.call(set_module_srv) == false) - { + if (set_joint_module_client.call(set_module_srv) == false) { ROS_ERROR("Failed to set module"); return; } - return ; + return; } -void torqueOnAll() -{ +void torqueOnAll() { std_msgs::String check_msg; check_msg.data = "check"; dxl_torque_pub.publish(check_msg); } -void torqueOff(const std::string& body_side) -{ +void torqueOff(const std::string &body_side) { robotis_controller_msgs::SyncWriteItem syncwrite_msg; int torque_value = 0; syncwrite_msg.item_name = "torque_enable"; - if(body_side == "right") - { + if (body_side == "right") { syncwrite_msg.joint_name.push_back("r_sho_pitch"); syncwrite_msg.value.push_back(torque_value); syncwrite_msg.joint_name.push_back("r_sho_roll"); syncwrite_msg.value.push_back(torque_value); syncwrite_msg.joint_name.push_back("r_el"); syncwrite_msg.value.push_back(torque_value); - } - else if(body_side == "left") - { + } else if (body_side == "left") { syncwrite_msg.joint_name.push_back("l_sho_pitch"); syncwrite_msg.value.push_back(torque_value); syncwrite_msg.joint_name.push_back("l_sho_roll"); syncwrite_msg.value.push_back(torque_value); syncwrite_msg.joint_name.push_back("l_el"); syncwrite_msg.value.push_back(torque_value); - } - else + } else return; sync_write_pub.publish(syncwrite_msg); diff --git a/robotis_op3_demo/CHANGELOG.rst b/robotis_op3_demo/CHANGELOG.rst index c761f4a..34b4ee4 100644 --- a/robotis_op3_demo/CHANGELOG.rst +++ b/robotis_op3_demo/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package robotis_op3_demo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.3.0 (2021-05-05) +------------------ +* Update package.xml and CMakeList.txt for noetic branch +* Contributors: Ronaldson Bellande + 0.1.0 (2018-04-19) ------------------ * first release for ROS Kinetic diff --git a/robotis_op3_demo/CMakeLists.txt b/robotis_op3_demo/CMakeLists.txt index b7e8eeb..aa20e05 100644 --- a/robotis_op3_demo/CMakeLists.txt +++ b/robotis_op3_demo/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.0.2) project(robotis_op3_demo) find_package(catkin REQUIRED) catkin_metapackage() diff --git a/robotis_op3_demo/package.xml b/robotis_op3_demo/package.xml index 83e6185..3922372 100644 --- a/robotis_op3_demo/package.xml +++ b/robotis_op3_demo/package.xml @@ -1,20 +1,34 @@ robotis_op3_demo - 0.1.0 + 0.3.0 ROS packages for the robotis_op3_demo (meta package) Apache 2.0 Kayman - Pyo + Ronaldson Bellande + http://wiki.ros.org/robotis_op3_demo http://emanual.robotis.com/docs/en/platform/op3/introduction/ https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues + catkin + + op3_ball_detector + op3_bringup + op3_demo + + op3_ball_detector + op3_bringup + op3_demo + op3_ball_detector op3_bringup op3_demo - + + + +