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

Merge pull request #1 from RonaldsonBellande/noetic

Browse Source 
noetic build
This commit is contained in:
Ronaldson Bellande
2022-05-06 15:10:32 -04:00
committed by GitHub
parent 77d76182b3 3dff932710
commit 11e987cedc
47 changed files with 2233 additions and 2454 deletions
Download Patch File Download Diff File Expand all files Collapse all files

117
op3_ball_detector/include/op3_ball_detector/ball_detector.h
View File

@@ -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 <string>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <dynamic_reconfigure/server.h>
#include <image_transport/image_transport.h>
#include <ros/package.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/image_encodings.h>
#include <dynamic_reconfigure/server.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <boost/thread.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/thread.hpp>
#include <yaml-cpp/yaml.h>
#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<op3_ball_detector::DetectorParamsConfig> param_server_;
dynamic_reconfigure::Server<op3_ball_detector::DetectorParamsConfig>::CallbackType callback_fnc_;
dynamic_reconfigure::Server<op3_ball_detector::DetectorParamsConfig>
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_

99
op3_ball_detector/include/op3_ball_detector/ball_detector_config.h
View File

@@ -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_

9
op3_ball_detector/launch/ball_detector.launch
View File

@@ -1,13 +1,12 @@
<?xml version="1.0"?>
<launch>
<arg name="config_path" default="$(find op3_ball_detector)/config/ball_detector_params.yaml"/>
<arg name="config_path" default="$(find op3_ball_detector)/config/ball_detector_params.yaml" />
<!-- ball detector -->
<node pkg="op3_ball_detector" type="ball_detector_node" name="ball_detector_node" args="" output="screen">
<rosparam command="load" file="$(arg config_path)"/>
<param name="yaml_path" type="string" value="$(arg config_path)"/>
<rosparam command="load" file="$(arg config_path)" />
<param name="yaml_path" type="string" value="$(arg config_path)" />
<remap from="/ball_detector_node/image_in" to="/usb_cam_node/image_raw" />
<remap from="/ball_detector_node/cameraInfo_in" to="/usb_cam_node/camera_info" />
</node>
</launch>

57
op3_ball_detector/launch/ball_detector_from_uvc.launch
View File

@@ -1,30 +1,29 @@
<?xml version="1.0"?>
<!-- Launches an UVC camera, the ball detector and its visualization -->
<launch>
<!-- UVC camera -->
<node pkg="uvc_camera" type="uvc_camera_node" name="uvc_camera_node" output="screen">
<param name="device" type="string" value="/dev/video0" />
<param name="width" type="int" value="800" />
<param name="height" type="int" value="600" />
<param name="fps" type="int" value="30" />
<param name="auto_gain" value="false" />
<param name="gain" type="int" value="120" />
<param name="exposure" value="100" />
</node>
<!-- <param name="gain" value="255" />
<param name="auto_exposure" type="bool" value="False" />
<param name="exposure_absolute" value="1000" />
<param name="brightness" value="127" />
<param name="auto_white_balance" type="bool" value="False" />
<param name="white_balance_temperature" value="2800" /> -->
<!-- <param name="auto_exposure" type="bool" value="False" /> -->
<!-- <param name="exposure_absolute" value="1000" /> -->
<!-- <param name="brightness" value="64" /> -->
<!-- <param name="auto_white_balance" type="bool" value="False" /> -->
<!-- <param name="white_balance_temperature" value="2800" /> -->
<!-- ball detector -->
<include file="$(find op3_ball_detector)/launch/ball_detector.launch" />
<?xml version="1.0"?>
<!-- Launches an UVC camera, the ball detector and its visualization -->
<launch>
<!-- UVC camera -->
<node pkg="uvc_camera" type="uvc_camera_node" name="uvc_camera_node" output="screen">
<param name="device" type="string" value="/dev/video0" />
<param name="width" type="int" value="800" />
<param name="height" type="int" value="600" />
<param name="fps" type="int" value="30" />
<param name="auto_gain" value="false" />
<param name="gain" type="int" value="120" />
<param name="exposure" value="100" />
</node>
<!-- <param name="gain" value="255" />
<param name="auto_exposure" type="bool" value="False" />

62
op3_ball_detector/package.xml
View File

@@ -10,26 +10,64 @@
<license>Apache 2.0</license>
<author email="kmjung@robotis.com">Kayman</author>
<author email="zerom@robotis.com">Zerom</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<url type="website">http://wiki.ros.org/op3_ball_detector</url>
<url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>roslib</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>dynamic_reconfigure</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<depend>boost</depend>
<depend>opencv3</depend>
<depend>yaml-cpp</depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>boost</build_depend>
<build_depend>opencv3</build_depend>
<build_depend>yaml-cpp</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>message_runtime</build_depend>
<build_depend>message_runtime</build_depend>
<build_depend>usb_cam</build_depend>
<build_depend>uvc_camera</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>roslib</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>dynamic_reconfigure</build_export_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>image_transport</build_export_depend>
<build_export_depend>boost</build_export_depend>
<build_export_depend>opencv3</build_export_depend>
<build_export_depend>yaml-cpp</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<build_export_depend>usb_cam</build_export_depend>
<build_export_depend>uvc_camera</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>roslib</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>dynamic_reconfigure</exec_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>image_transport</exec_depend>
<exec_depend>boost</exec_depend>
<exec_depend>opencv3</exec_depend>
<exec_depend>yaml-cpp</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>usb_cam</exec_depend>
<exec_depend>uvc_camera</exec_depend>
</package>

651
op3_ball_detector/src/ball_detector.cpp
View File

File diff suppressed because it is too large Load Diff

57
op3_ball_detector/src/ball_detector_node.cpp
View File

@@ -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;
}

5
op3_bringup/CHANGELOG.rst
View File

@@ -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

9
op3_bringup/CMakeLists.txt
View File

@@ -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
################################################################################
################################################################################

8
op3_bringup/launch/op3_bringup.launch
View File

@@ -1,8 +1,8 @@
<?xml version="1.0" ?>
<launch>
<?xml version="1.0"?>
<launch>
<!-- OP3 Manager -->
<include file="$(find op3_manager)/launch/op3_manager.launch" />
<include file="$(find op3_manager)/launch/op3_manager.launch" />
<!-- UVC camera -->
<node pkg="usb_cam" type="usb_cam_node" name="usb_cam_node" output="screen">
<param name="video_device" type="string" value="/dev/video0" />

8
op3_bringup/launch/op3_bringup_visualization.launch
View File

@@ -1,10 +1,10 @@
<?xml version="1.0" ?>
<?xml version="1.0"?>
<launch>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find op3_description)/urdf/robotis_op3.urdf.xacro'"/>
<param name="robot_description" command="$(find xacro)/xacro.py '$(find op3_description)/urdf/robotis_op3.urdf.xacro'" />
<!-- Send fake joint values and monitoring present joint angle -->
<node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
<param name="use_gui" value="TRUE"/>
<param name="use_gui" value="TRUE" />
<rosparam param="/source_list">[/robotis/present_joint_states]</rosparam>
</node>
@@ -14,5 +14,5 @@
</node>
<!-- Show in Rviz -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find op3_bringup)/rviz/op3_bringup.rviz"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find op3_bringup)/rviz/op3_bringup.rviz" />
</launch>

24
op3_bringup/package.xml
View File

@@ -1,23 +1,41 @@
<?xml version="1.0"?>
<package format="2">
<name>op3_bringup</name>
<version>0.1.0</version>
<version>0.3.0</version>
<description>
This package is a demo for first time users.
There is an example in the demo where you can run and visualize the robot.
</description>
<license>Apache 2.0</license>
<author email="kmjung@robotis.com">Kayman</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<url type="website">http://wiki.ros.org/op3_bringup</url>
<url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>op3_manager</build_depend>
<build_depend>op3_description</build_depend>
<build_depend>usb_cam</build_depend>
<build_depend>joint_state_publisher</build_depend>
<build_depend>robot_state_publisher</build_depend>
<build_depend>rviz</build_depend>
<build_export_depend>op3_manager</build_export_depend>
<build_export_depend>op3_description</build_export_depend>
<build_export_depend>usb_cam</build_export_depend>
<build_export_depend>joint_state_publisher</build_export_depend>
<build_export_depend>robot_state_publisher</build_export_depend>
<build_export_depend>rviz</build_export_depend>
<exec_depend>op3_manager</exec_depend>
<exec_depend>op3_description</exec_depend>
<exec_depend>usb_cam</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>rviz</exec_depend>
<exec_depend>rviz</exec_depend>
</package>

5
op3_demo/CHANGELOG.rst
View File

@@ -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

64
op3_demo/CMakeLists.txt
View File

@@ -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 <yaml-cpp/yaml.h>'
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}
)
################################################################################

55
op3_demo/include/op3_demo/action_demo.h
View File

@@ -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 <ros/ros.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <boost/thread.hpp>
#include <yaml-cpp/yaml.h>
#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_;

87
op3_demo/include/op3_demo/ball_follower.h
View File

@@ -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 <geometry_msgs/Point.h>
#include <math.h>
#include <numeric>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#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_ */

74
op3_demo/include/op3_demo/ball_tracker.h
View File

@@ -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 <math.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Point.h>
#include <math.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#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_ */

46
op3_demo/include/op3_demo/button_test.h
View File

@@ -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 <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <boost/thread.hpp>
#include <ros/package.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#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_;

72
op3_demo/include/op3_demo/face_tracker.h
View File

@@ -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 <math.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32.h>
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/Point.h>
#include <math.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
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_ */

51
op3_demo/include/op3_demo/mic_test.h
View File

@@ -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 <signal.h>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/String.h>
#include <boost/thread.hpp>
#include <ros/package.h>
#include <ros/ros.h>
#include <signal.h>
#include <std_msgs/String.h>
#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_;

57
op3_demo/include/op3_demo/op_demo.h
View File

@@ -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_;
};

70
op3_demo/include/op3_demo/soccer_demo.h
View File

@@ -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 <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Imu.h>
#include <boost/thread.hpp>
#include <eigen3/Eigen/Eigen>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/String.h>
#include <yaml-cpp/yaml.h>
#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

54
op3_demo/include/op3_demo/vision_demo.h
View File

@@ -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 <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32MultiArray.h>
#include <geometry_msgs/Point.h>
#include <boost/thread.hpp>
#include <geometry_msgs/Point.h>
#include <ros/ros.h>
#include <std_msgs/Int32MultiArray.h>
#include <std_msgs/String.h>
#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_;

45
op3_demo/launch/demo.launch
View File

@@ -1,28 +1,27 @@
<?xml version="1.0"?>
<launch>
<!-- robotis op3 manager -->
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
<!-- robotis op3 manager -->
<include file="$(find op3_manager)/launch/op3_manager.launch" />
<!-- Camera and Ball detector -->
<include file="$(find op3_ball_detector)/launch/ball_detector_from_usb_cam.launch"/>
<!-- face tracking -->
<include file="$(find op3_demo)/launch/face_detection_op3.launch" />
<!-- Camera and Ball detector -->
<include file="$(find op3_ball_detector)/launch/ball_detector_from_usb_cam.launch" />
<!-- camera setting tool -->
<include file="$(find op3_camera_setting_tool)/launch/op3_camera_setting_tool.launch" />
<!-- sound player -->
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
<!-- web setting -->
<include file="$(find op3_web_setting_tool)/launch/web_setting_server.launch" />
<!-- robotis op3 demo -->
<node pkg="op3_demo" type="op_demo_node" name="op3_demo" output="screen">
<param name="grass_demo" type="bool" value="False" />
<param name="p_gain" value="0.45" />
<param name="d_gain" value="0.045" />
</node>
<!-- face tracking -->
<include file="$(find op3_demo)/launch/face_detection_op3.launch" />
<!-- camera setting tool -->
<include file="$(find op3_camera_setting_tool)/launch/op3_camera_setting_tool.launch" />
<!-- sound player -->
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen" />
<!-- web setting -->
<include file="$(find op3_web_setting_tool)/launch/web_setting_server.launch" />
<!-- robotis op3 demo -->
<node pkg="op3_demo" type="op_demo_node" name="op3_demo" output="screen">
<param name="grass_demo" type="bool" value="False" />
<param name="p_gain" value="0.45" />
<param name="d_gain" value="0.045" />
</node>
</launch>

18
op3_demo/launch/face_detection_op3.launch
View File

@@ -1,22 +1,20 @@
<?xml version="1.0"?>
<launch>
<arg name="face_cascade_name_0" default="$(find face_detection)/include/face_detection/HaarCascades/haarcascade_frontalface_alt.xml" />
<arg name="face_cascade_name_1" default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_alt2.xml" />
<arg name="face_cascade_name_2" default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_alt_tree.xml" />
<arg name="face_cascade_name_3" default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_default.xml" />
<arg name="face_cascade_name_4" default="$(find face_detection)/include/face_detection/lbpCascades/lbpcascade_frontalface.xml" />
<arg name="face_cascade_name_0" default="$(find face_detection)/include/face_detection/HaarCascades/haarcascade_frontalface_alt.xml" />
<arg name="face_cascade_name_1" default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_alt2.xml" />
<arg name="face_cascade_name_2" default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_alt_tree.xml" />
<arg name="face_cascade_name_3" default="$(find face_detection)/include/face_detection//HaarCascades/haarcascade_frontalface_default.xml" />
<arg name="face_cascade_name_4" default="$(find face_detection)/include/face_detection/lbpCascades/lbpcascade_frontalface.xml" />
<node pkg="face_detection" type="face_tracking" name="face_tracking"
args="$(arg face_cascade_name_0)
<node pkg="face_detection" type="face_tracking" name="face_tracking" args="$(arg face_cascade_name_0)
$(arg face_cascade_name_1)
$(arg face_cascade_name_2)
$(arg face_cascade_name_3)
$(arg face_cascade_name_4)"
output="screen">
$(arg face_cascade_name_4)" output="screen">
<param name="imageInput" type="string" value="/usb_cam_node/image_raw" />
<param name="displayed_Image" type="int" value="0" />
<!-- <param name="publish" type="int" value="2" /> -->
<param name="publish" type="int" value="3" />
<param name="start_condition" type="bool" value="false" />
<param name="start_condition" type="bool" value="false" />
</node>
</launch>

17
op3_demo/launch/self_test.launch
View File

@@ -1,24 +1,24 @@
<?xml version="1.0"?>
<!-- Launches an UVC camera, the ball detector and its visualization -->
<launch>
<launch>
<!-- robotis op3 manager -->
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
<include file="$(find op3_manager)/launch/op3_manager.launch" />
<!-- Camera and Ball detector -->
<include file="$(find op3_ball_detector)/launch/ball_detector_from_usb_cam.launch"/>
<include file="$(find op3_ball_detector)/launch/ball_detector_from_usb_cam.launch" />
<!-- face tracking -->
<include file="$(find op3_demo)/launch/face_detection_op3.launch" />
<!-- camera setting tool -->
<include file="$(find op3_camera_setting_tool)/launch/op3_camera_setting_tool.launch" />
<!-- sound player -->
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen"/>
<node pkg="ros_madplay_player" type="ros_madplay_player" name="ros_madplay_player" output="screen" />
<!-- web setting -->
<include file="$(find op3_web_setting_tool)/launch/web_setting_server.launch" />
<!-- robotis op3 self test demo -->
<node pkg="op3_demo" type="self_test_node" name="op3_self_test" output="screen">
<param name="grass_demo" type="bool" value="False" />
@@ -26,4 +26,3 @@
<param name="d_gain" value="0.045" />
</node>
</launch>

2
op3_demo/list/action_script.yaml
View File

@@ -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]
#certification: [101]

2
op3_demo/list/action_script_bk.yaml
View File

@@ -15,4 +15,4 @@ action_and_sound:
default: [4, 41, 24, 23, 15, 1, 54, 27, 38]
# example of play list
certificatino: [101]
certificatino: [101]

78
op3_demo/package.xml
View File

@@ -1,36 +1,80 @@
<?xml version="1.0"?>
<package format="2">
<name>op3_demo</name>
<version>0.1.0</version>
<version>0.3.0</version>
<description>
OP3 default demo
It includes three demontrations(soccer demo, vision demo, action script demo)
</description>
<license>Apache 2.0</license>
<author email="kmjung@robotis.com">Kayman</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<url type="website">http://wiki.ros.org/op3_demo</url>
<url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>roslib</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<depend>robotis_controller_msgs</depend>
<depend>op3_walking_module_msgs</depend>
<depend>op3_action_module_msgs</depend>
<depend>cmake_modules</depend>
<depend>robotis_math</depend>
<depend>op3_ball_detector</depend>
<depend>boost</depend>
<depend>eigen</depend>
<depend>yaml-cpp</depend>
<build_depend>roscpp</build_depend>
<build_depend>roslib</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>robotis_controller_msgs</build_depend>
<build_depend>op3_walking_module_msgs</build_depend>
<build_depend>op3_action_module_msgs</build_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>robotis_math</build_depend>
<build_depend>op3_ball_detector</build_depend>
<build_depend>boost</build_depend>
<build_depend>eigen</build_depend>
<build_depend>yaml-cpp</build_depend>
<build_depend>op3_manager</build_depend>
<build_depend>op3_camera_setting_tool</build_depend>
<build_depend>op3_web_setting_tool</build_depend>
<build_depend>ros_madplay_player</build_depend>
<build_depend>face_detection</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>roslib</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>robotis_controller_msgs</build_export_depend>
<build_export_depend>op3_walking_module_msgs</build_export_depend>
<build_export_depend>op3_action_module_msgs</build_export_depend>
<build_export_depend>cmake_modules</build_export_depend>
<build_export_depend>robotis_math</build_export_depend>
<build_export_depend>op3_ball_detector</build_export_depend>
<build_export_depend>boost</build_export_depend>
<build_export_depend>eigen</build_export_depend>
<build_export_depend>yaml-cpp</build_export_depend>
<build_export_depend>op3_manager</build_export_depend>
<build_export_depend>op3_camera_setting_tool</build_export_depend>
<build_export_depend>op3_web_setting_tool</build_export_depend>
<build_export_depend>ros_madplay_player</build_export_depend>
<build_export_depend>face_detection</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>roslib</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>robotis_controller_msgs</exec_depend>
<exec_depend>op3_walking_module_msgs</exec_depend>
<exec_depend>op3_action_module_msgs</exec_depend>
<exec_depend>cmake_modules</exec_depend>
<exec_depend>robotis_math</exec_depend>
<exec_depend>op3_ball_detector</exec_depend>
<exec_depend>boost</exec_depend>
<exec_depend>eigen</exec_depend>
<exec_depend>yaml-cpp</exec_depend>
<exec_depend>op3_manager</exec_depend>
<exec_depend>op3_camera_setting_tool</exec_depend>
<exec_depend>op3_web_setting_tool</exec_depend>
<exec_depend>ros_madplay_player</exec_depend>
<!-- <exec_depend>face_detection</exec_depend> -->
<exec_depend>face_detection</exec_depend>
</package>

345
op3_demo/src/action/action_demo.cpp
View File

@@ -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<std::string>("action_script", default_path);
std::string default_play_list = "default";
play_list_name_ = nh.param<std::string>("action_script_play_list", default_play_list);
play_list_name_ =
nh.param<std::string>("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<std_msgs::String>("/robotis/enable_ctrl_module", 0);
motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
module_control_pub_ =
nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
motion_index_pub_ =
nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
play_sound_pub_ = nh.advertise<std_msgs::String>("/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<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");
set_joint_module_client_ = nh.serviceClient<robotis_controller_msgs::SetModule>("/robotis/set_present_ctrl_modules");
is_running_client_ = nh.serviceClient<op3_action_module_msgs::IsRunning>(
"/robotis/action/is_running");
set_joint_module_client_ =
nh.serviceClient<robotis_controller_msgs::SetModule>(
"/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<int>();
std::string mp3_path = yaml_it->second.as<std::string>();
@@ -216,82 +195,75 @@ void ActionDemo::parseActionScript(const std::string &path)
// default action set
if (doc["default"])
play_list_ = doc["default"].as<std::vector<int> >();
play_list_ = doc["default"].as<std::vector<int>>();
}
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<std::vector<int> >();
if (doc[set_name]) {
play_list_ = doc[set_name].as<std::vector<int>>();
return true;
}
else
} else
return false;
}
bool ActionDemo::playActionWithSound(int motion_index)
{
std::map<int, std::string>::iterator map_it = action_sound_table_.find(motion_index);
bool ActionDemo::playActionWithSound(int motion_index) {
std::map<int, std::string>::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();
}
}

294
op3_demo/src/demo_node.cpp
View File

@@ -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 <ros/ros.h>
#include <std_msgs/String.h>
#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<std_msgs::String>("/robotis/base/ini_pose", 0);
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
led_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
led_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>(
"/robotis/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>("/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<std::string> 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)
}
}
}

251
op3_demo/src/soccer/ball_follower.cpp
View File

@@ -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<std_msgs::String>("/robotis/walking/command", 0);
set_walking_param_pub_ = nh_.advertise<op3_walking_module_msgs::WalkingParam>("/robotis/walking/set_params", 0);
get_walking_param_client_ = nh_.serviceClient<op3_walking_module_msgs::GetWalkingParam>(
"/robotis/walking/get_params");
set_walking_command_pub_ =
nh_.advertise<std_msgs::String>("/robotis/walking/command", 0);
set_walking_param_pub_ = nh_.advertise<op3_walking_module_msgs::WalkingParam>(
"/robotis/walking/set_params", 0);
get_walking_param_client_ =
nh_.serviceClient<op3_walking_module_msgs::GetWalkingParam>(
"/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

201
op3_demo/src/soccer/ball_tracker.cpp
View File

@@ -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<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states", 0);
head_scan_pub_ = nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
// error_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/ball_tracker/errors", 0);
head_joint_offset_pub_ = nh_.advertise<sensor_msgs::JointState>(
"/robotis/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>(
"/robotis/head_control/set_joint_states", 0);
head_scan_pub_ =
nh_.advertise<std_msgs::String>("/robotis/head_control/scan_command", 0);
// error_pub_ =
// nh_.advertise<std_msgs::Float64MultiArray>("/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

354
op3_demo/src/soccer/soccer_demo.cpp
View File

@@ -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<std::string>("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<bool>("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_controller_msgs::JointCtrlModule>("/robotis/set_joint_ctrl_modules", 0);
motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
module_control_pub_ = nh.advertise<robotis_controller_msgs::JointCtrlModule>(
"/robotis/set_joint_ctrl_modules", 0);
motion_index_pub_ =
nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>(
"/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<op3_action_module_msgs::IsRunning>("/robotis/action/is_running");
set_joint_module_client_ = nh.serviceClient<robotis_controller_msgs::SetJointModule>("/robotis/set_present_joint_ctrl_modules");
is_running_client_ = nh.serviceClient<op3_action_module_msgs::IsRunning>(
"/robotis/action/is_running");
set_joint_module_client_ =
nh.serviceClient<robotis_controller_msgs::SetJointModule>(
"/robotis/set_present_joint_ctrl_modules");
test_pub_ = nh.advertise<std_msgs::String>("/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<int, std::string>::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<int, std::string>::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<int, std::string>::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<std::string, int>::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<std::string, int>::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

127
op3_demo/src/test/button_test.cpp
View File

@@ -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_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>(
"/robotis/sync_write_item", 0);
play_sound_pub_ = nh.advertise<std_msgs::String>("/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;

229
op3_demo/src/test/mic_test.cpp
View File

@@ -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<std_msgs::String>("/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 */

365
op3_demo/src/test_node.cpp
View File

@@ -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 <ros/ros.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#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<std_msgs::String>("/robotis/base/ini_pose", 0);
play_sound_pub = nh.advertise<std_msgs::String>("/play_sound_file", 0);
led_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
led_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>(
"/robotis/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>("/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<std::string> 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)
}
}
}

141
op3_demo/src/vision/face_tracker.cpp
View File

@@ -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<sensor_msgs::JointState>("/robotis/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>("/robotis/head_control/set_joint_states", 0);
head_scan_pub_ = nh_.advertise<std_msgs::String>("/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<sensor_msgs::JointState>(
"/robotis/head_control/set_joint_states_offset", 0);
head_joint_pub_ = nh_.advertise<sensor_msgs::JointState>(
"/robotis/head_control/set_joint_states", 0);
head_scan_pub_ =
nh_.advertise<std_msgs::String>("/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

174
op3_demo/src/vision/vision_demo.cpp
View File

@@ -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<std_msgs::String>("/robotis/enable_ctrl_module", 0);
motion_index_pub_ = nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
face_tracking_command_pub_ = nh.advertise<std_msgs::Bool>("/face_tracking/command", 0);
module_control_pub_ =
nh.advertise<std_msgs::String>("/robotis/enable_ctrl_module", 0);
motion_index_pub_ =
nh.advertise<std_msgs::Int32>("/robotis/action/page_num", 0);
rgb_led_pub_ = nh.advertise<robotis_controller_msgs::SyncWriteItem>(
"/robotis/sync_write_item", 0);
face_tracking_command_pub_ =
nh.advertise<std_msgs::Bool>("/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_controller_msgs::SetModule>("/robotis/set_present_ctrl_modules");
set_joint_module_client_ =
nh.serviceClient<robotis_controller_msgs::SetModule>(
"/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");

8
op3_read_write_demo/CHANGELOG.rst Normal file
View File

@@ -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

36
op3_read_write_demo/CMakeLists.txt
View File

@@ -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}
)
################################################################################

32
op3_read_write_demo/launch/op3_read_write.launch
View File

@@ -1,24 +1,24 @@
<?xml version="1.0" ?>
<launch>
<param name="gazebo" value="false" type="bool"/>
<param name="gazebo_robot_name" value="robotis_op3"/>
<param name="offset_file_path" value="$(find op3_manager)/config/offset.yaml"/>
<param name="robot_file_path" value="$(find op3_manager)/config/OP3.robot"/>
<param name="init_file_path" value="$(find op3_manager)/config/dxl_init_OP3.yaml"/>
<param name="device_name" value="/dev/ttyUSB0"/>
<param name="/robotis/direct_control/default_moving_time" value="0.04"/>
<param name="/robotis/direct_control/default_moving_angle" value="90"/>
<?xml version="1.0"?>
<launch>
<param name="gazebo" value="false" type="bool" />
<param name="gazebo_robot_name" value="robotis_op3" />
<param name="offset_file_path" value="$(find op3_manager)/config/offset.yaml" />
<param name="robot_file_path" value="$(find op3_manager)/config/OP3.robot" />
<param name="init_file_path" value="$(find op3_manager)/config/dxl_init_OP3.yaml" />
<param name="device_name" value="/dev/ttyUSB0" />
<param name="/robotis/direct_control/default_moving_time" value="0.04" />
<param name="/robotis/direct_control/default_moving_angle" value="90" />
<!-- OP3 Manager -->
<node pkg="op3_manager" type="op3_manager" name="op3_manager" output="screen">
<param name="angle_unit" value="30" />
</node>
<!-- OP3 Localization -->
<node pkg="op3_localization" type="op3_localization" name="op3_localization" output="screen"/>
<node pkg="op3_localization" type="op3_localization" name="op3_localization" output="screen" />
<!-- OP3 Read-Write demo -->
<node pkg="op3_read_write_demo" type="read_write" name="op3_read_write" output="screen"/>
<node pkg="op3_read_write_demo" type="read_write" name="op3_read_write" output="screen" />
</launch>

29
op3_read_write_demo/package.xml
View File

@@ -1,23 +1,38 @@
<?xml version="1.0"?>
<package format="2">
<name>op3_read_write_demo</name>
<version>0.0.1</version>
<version>0.3.0</version>
<description>
The op3_read_write_demo package
</description>
<license>Apache 2.0</license>
<author email="kmjung@robotis.com">Kayman</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<maintainer email="pyo@robotis.com">Ronaldson Bellande</maintainer>
<url type="website">http://wiki.ros.org/op3_demo</url>
<url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>robotis_controller_msgs</depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>robotis_controller_msgs</build_depend>
<build_depend>op3_manager</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>robotis_controller_msgs</build_export_depend>
<build_export_depend>op3_manager</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>robotis_controller_msgs</exec_depend>
<exec_depend>op3_manager</exec_depend>
</package>

171
op3_read_write_demo/src/read_write.cpp
View File

@@ -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 <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
#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<std_msgs::String>("/robotis/base/ini_pose", 0);
sync_write_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>("/robotis/sync_write_item", 0);
sync_write_pub = nh.advertise<robotis_controller_msgs::SyncWriteItem>(
"/robotis/sync_write_item", 0);
dxl_torque_pub = nh.advertise<std_msgs::String>("/robotis/dxl_torque", 0);
write_joint_pub = nh.advertise<sensor_msgs::JointState>("/robotis/set_joint_states", 0);
write_joint_pub2 = nh.advertise<sensor_msgs::JointState>("/robotis/direct_control/set_joint_states", 0);
write_joint_pub =
nh.advertise<sensor_msgs::JointState>("/robotis/set_joint_states", 0);
write_joint_pub2 = nh.advertise<sensor_msgs::JointState>(
"/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_controller_msgs::SetModule>("/robotis/set_present_ctrl_modules");
set_joint_module_client =
nh.serviceClient<robotis_controller_msgs::SetModule>(
"/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<std::string> 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);

5
robotis_op3_demo/CHANGELOG.rst
View File

@@ -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

2
robotis_op3_demo/CMakeLists.txt
View File

@@ -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()

20
robotis_op3_demo/package.xml
View File

@@ -1,20 +1,34 @@
<?xml version="1.0"?>
<package format="2">
<name>robotis_op3_demo</name>
<version>0.1.0</version>
<version>0.3.0</version>
<description>
ROS packages for the robotis_op3_demo (meta package)
</description>
<license>Apache 2.0</license>
<author email="kmjung@robotis.com">Kayman</author>
<maintainer email="pyo@robotis.com">Pyo</maintainer>
<maintainer email="ronaldsonbellande@gmail.com">Ronaldson Bellande</maintainer>
<url type="website">http://wiki.ros.org/robotis_op3_demo</url>
<url type="emanual">http://emanual.robotis.com/docs/en/platform/op3/introduction/</url>
<url type="repository">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo</url>
<url type="bugtracker">https://github.com/ROBOTIS-GIT/ROBOTIS-OP3-Demo/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>op3_ball_detector</build_depend>
<build_depend>op3_bringup</build_depend>
<build_depend>op3_demo</build_depend>
<build_export_depend>op3_ball_detector</build_export_depend>
<build_export_depend>op3_bringup</build_export_depend>
<build_export_depend>op3_demo</build_export_depend>
<exec_depend>op3_ball_detector</exec_depend>
<exec_depend>op3_bringup</exec_depend>
<exec_depend>op3_demo</exec_depend>
<export><metapackage/></export>
<export>
<metapackage />
</export>
</package>