Merge pull request #18 from ROBOTIS-GIT/master
merge for sync kinetic-devel and master branch
This commit is contained in:
commit
e101b4d727
@ -1,4 +1,4 @@
|
||||
## ROBOTIS OP3
|
||||
# ROBOTIS OP3
|
||||
<img src="https://github.com/ROBOTIS-GIT/emanual/blob/master/assets/images/platform/op3/default_op3.jpg" width="40%" />
|
||||
|
||||
## ROS Packages for ROBOTIS OP3 Demo
|
||||
|
@ -2,7 +2,7 @@
|
||||
# Set minimum required version of cmake, project name and compile options
|
||||
################################################################################
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(ball_detector)
|
||||
project(op3_ball_detector)
|
||||
|
||||
################################################################################
|
||||
# Find catkin packages and libraries for catkin and system dependencies
|
@ -1,5 +1,5 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE='ball_detector'
|
||||
PACKAGE='op3_ball_detector'
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
@ -1,5 +1,5 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE='ball_detector'
|
||||
PACKAGE='op3_ball_detector'
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
@ -1,5 +1,5 @@
|
||||
#!/usr/bin/env python
|
||||
PACKAGE='ball_detector'
|
||||
PACKAGE='op3_ball_detector'
|
||||
|
||||
from dynamic_reconfigure.parameter_generator_catkin import *
|
||||
|
@ -36,13 +36,12 @@
|
||||
#include <boost/thread.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include "ball_detector/DetectorParamsConfig.h"
|
||||
#include "op3_ball_detector/ball_detector_config.h"
|
||||
|
||||
#include "ball_detector/ball_detector_config.h"
|
||||
|
||||
#include "ball_detector/CircleSetStamped.h"
|
||||
#include "ball_detector/GetParameters.h"
|
||||
#include "ball_detector/SetParameters.h"
|
||||
#include "op3_ball_detector/DetectorParamsConfig.h"
|
||||
#include "op3_ball_detector/CircleSetStamped.h"
|
||||
#include "op3_ball_detector/GetParameters.h"
|
||||
#include "op3_ball_detector/SetParameters.h"
|
||||
|
||||
namespace robotis_op
|
||||
{
|
||||
@ -74,12 +73,12 @@ class BallDetector
|
||||
//callbacks to camera info subscription
|
||||
void cameraInfoCallback(const sensor_msgs::CameraInfo & msg);
|
||||
|
||||
void dynParamCallback(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(ball_detector::SetParameters::Request &req, ball_detector::SetParameters::Response &res);
|
||||
bool getParamCallback(ball_detector::GetParameters::Request &req, 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();
|
||||
|
||||
@ -115,7 +114,7 @@ class BallDetector
|
||||
int not_found_count_;
|
||||
|
||||
//circle set publisher
|
||||
ball_detector::CircleSetStamped circles_msg_;
|
||||
op3_ball_detector::CircleSetStamped circles_msg_;
|
||||
ros::Publisher circles_pub_;
|
||||
|
||||
//camera info subscriber
|
||||
@ -157,8 +156,8 @@ class BallDetector
|
||||
cv::Mat in_image_;
|
||||
cv::Mat out_image_;
|
||||
|
||||
dynamic_reconfigure::Server<ball_detector::DetectorParamsConfig> param_server_;
|
||||
dynamic_reconfigure::Server<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
|
@ -1,9 +1,9 @@
|
||||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<arg name="config_path" default="$(find 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="ball_detector" type="ball_detector_node" name="ball_detector_node" args="" output="screen">
|
||||
<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)"/>
|
||||
<remap from="/ball_detector_node/image_in" to="/usb_cam_node/image_raw" />
|
@ -23,5 +23,5 @@
|
||||
</node>
|
||||
|
||||
<!-- ball detector -->
|
||||
<include file="$(find ball_detector)/launch/ball_detector.launch" />
|
||||
<include file="$(find op3_ball_detector)/launch/ball_detector.launch" />
|
||||
</launch>
|
@ -25,6 +25,6 @@
|
||||
<!-- <param name="white_balance_temperature" value="2800" /> -->
|
||||
|
||||
<!-- ball detector -->
|
||||
<include file="$(find ball_detector)/launch/ball_detector.launch" />
|
||||
<include file="$(find op3_ball_detector)/launch/ball_detector.launch" />
|
||||
</launch>
|
||||
|
@ -1,6 +1,6 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>ball_detector</name>
|
||||
<name>op3_ball_detector</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
This package implements a circle-like shape detector of the input image.
|
@ -18,7 +18,7 @@
|
||||
|
||||
#include <fstream>
|
||||
|
||||
#include "ball_detector/ball_detector.h"
|
||||
#include "op3_ball_detector/ball_detector.h"
|
||||
|
||||
namespace robotis_op
|
||||
{
|
||||
@ -71,7 +71,7 @@ BallDetector::BallDetector()
|
||||
|
||||
//sets publishers
|
||||
image_pub_ = it_.advertise("image_out", 100);
|
||||
circles_pub_ = nh_.advertise<ball_detector::CircleSetStamped>("circle_set", 100);
|
||||
circles_pub_ = nh_.advertise<op3_ball_detector::CircleSetStamped>("circle_set", 100);
|
||||
camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>("camera_info", 100);
|
||||
|
||||
//sets subscribers
|
||||
@ -87,7 +87,7 @@ BallDetector::BallDetector()
|
||||
param_server_.setCallback(callback_fnc_);
|
||||
|
||||
// web setting
|
||||
param_pub_ = nh_.advertise<ball_detector::BallDetectorParams>("current_params", 1);
|
||||
param_pub_ = nh_.advertise<op3_ball_detector::BallDetectorParams>("current_params", 1);
|
||||
param_command_sub_ = nh_.subscribe("param_command", 1, &BallDetector::paramCommandCallback, this);
|
||||
set_param_client_ = nh_.advertiseService("set_param", &BallDetector::setParamCallback, this);
|
||||
get_param_client_ = nh_.advertiseService("get_param", &BallDetector::getParamCallback, this);
|
||||
@ -233,7 +233,7 @@ void BallDetector::imageCallback(const sensor_msgs::ImageConstPtr & msg)
|
||||
return;
|
||||
}
|
||||
|
||||
void BallDetector::dynParamCallback(ball_detector::DetectorParamsConfig &config, uint32_t level)
|
||||
void BallDetector::dynParamCallback(op3_ball_detector::DetectorParamsConfig &config, uint32_t level)
|
||||
{
|
||||
params_config_.gaussian_blur_size = config.gaussian_blur_size;
|
||||
params_config_.gaussian_blur_sigma = config.gaussian_blur_sigma;
|
||||
@ -296,7 +296,7 @@ void BallDetector::paramCommandCallback(const std_msgs::String::ConstPtr &msg)
|
||||
}
|
||||
}
|
||||
|
||||
bool BallDetector::setParamCallback(ball_detector::SetParameters::Request &req, ball_detector::SetParameters::Response &res)
|
||||
bool BallDetector::setParamCallback(op3_ball_detector::SetParameters::Request &req, op3_ball_detector::SetParameters::Response &res)
|
||||
{
|
||||
params_config_.gaussian_blur_size = req.params.gaussian_blur_size;
|
||||
params_config_.gaussian_blur_sigma = req.params.gaussian_blur_sigma;
|
||||
@ -321,7 +321,7 @@ bool BallDetector::setParamCallback(ball_detector::SetParameters::Request &req,
|
||||
return true;
|
||||
}
|
||||
|
||||
bool BallDetector:: getParamCallback(ball_detector::GetParameters::Request &req, ball_detector::GetParameters::Response &res)
|
||||
bool BallDetector:: getParamCallback(op3_ball_detector::GetParameters::Request &req, op3_ball_detector::GetParameters::Response &res)
|
||||
{
|
||||
res.returns.gaussian_blur_size = params_config_.gaussian_blur_size;
|
||||
res.returns.gaussian_blur_sigma = params_config_.gaussian_blur_sigma;
|
||||
@ -396,7 +396,7 @@ void BallDetector::resetParameter()
|
||||
|
||||
void BallDetector::publishParam()
|
||||
{
|
||||
ball_detector::BallDetectorParams params;
|
||||
op3_ball_detector::BallDetectorParams params;
|
||||
|
||||
params.gaussian_blur_size = params_config_.gaussian_blur_size;
|
||||
params.gaussian_blur_sigma = params_config_.gaussian_blur_sigma;
|
@ -16,7 +16,7 @@
|
||||
|
||||
/* Author: Kayman Jung */
|
||||
|
||||
#include "ball_detector/ball_detector.h"
|
||||
#include "op3_ball_detector/ball_detector.h"
|
||||
|
||||
//node main
|
||||
int main(int argc, char **argv)
|
42
op3_bringup/CMakeLists.txt
Normal file
42
op3_bringup/CMakeLists.txt
Normal file
@ -0,0 +1,42 @@
|
||||
################################################################################
|
||||
# Set minimum required version of cmake, project name and compile options
|
||||
################################################################################
|
||||
cmake_minimum_required(VERSION 2.8.3)
|
||||
project(op3_bringup)
|
||||
|
||||
################################################################################
|
||||
# Find catkin packages and libraries for catkin and system dependencies
|
||||
################################################################################
|
||||
find_package(catkin REQUIRED)
|
||||
|
||||
################################################################################
|
||||
# Setup for python modules and scripts
|
||||
################################################################################
|
||||
|
||||
################################################################################
|
||||
# Declare ROS messages, services and actions
|
||||
################################################################################
|
||||
|
||||
################################################################################
|
||||
# Declare ROS dynamic reconfigure parameters
|
||||
################################################################################
|
||||
|
||||
################################################################################
|
||||
# Declare catkin specific configuration to be passed to dependent projects
|
||||
################################################################################
|
||||
catkin_package()
|
||||
|
||||
################################################################################
|
||||
# Build
|
||||
################################################################################
|
||||
|
||||
################################################################################
|
||||
# Install
|
||||
################################################################################
|
||||
install(DIRECTORY launch rviz
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
################################################################################
|
||||
# Test
|
||||
################################################################################
|
15
op3_bringup/launch/op3_bringup.launch
Normal file
15
op3_bringup/launch/op3_bringup.launch
Normal file
@ -0,0 +1,15 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<!-- OP3 Manager -->
|
||||
<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" />
|
||||
<param name="image_width" type="int" value="1280" />
|
||||
<param name="image_height" type="int" value="720" />
|
||||
<param name="framerate " type="int" value="30" />
|
||||
<param name="camera_frame_id" type="string" value="cam_link" />
|
||||
<param name="camera_name" type="string" value="camera" />
|
||||
</node>
|
||||
</launch>
|
18
op3_bringup/launch/op3_bringup_visualization.launch
Normal file
18
op3_bringup/launch/op3_bringup_visualization.launch
Normal file
@ -0,0 +1,18 @@
|
||||
<?xml version="1.0" ?>
|
||||
<launch>
|
||||
<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"/>
|
||||
<rosparam param="/source_list">[/robotis/present_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Combine joint values -->
|
||||
<node pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
|
||||
<remap from="/joint_states" to="/robotis/present_joint_states" />
|
||||
</node>
|
||||
|
||||
<!-- Show in Rviz -->
|
||||
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find op3_bringup)/rviz/op3_bringup.rviz"/>
|
||||
</launch>
|
22
op3_bringup/package.xml
Normal file
22
op3_bringup/package.xml
Normal file
@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>op3_bringup</name>
|
||||
<version>0.0.1</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>
|
||||
<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>
|
||||
<exec_depend>op3_manager</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>
|
||||
</package>
|
357
op3_bringup/rviz/op3_bringup.rviz
Normal file
357
op3_bringup/rviz/op3_bringup.rviz
Normal file
@ -0,0 +1,357 @@
|
||||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 78
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /Global Options1
|
||||
- /Status1
|
||||
- /TF1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 352
|
||||
- Class: rviz/Selection
|
||||
Name: Selection
|
||||
- Class: rviz/Tool Properties
|
||||
Expanded:
|
||||
- /2D Pose Estimate1
|
||||
- /2D Nav Goal1
|
||||
- /Publish Point1
|
||||
Name: Tool Properties
|
||||
Splitter Ratio: 0.588679016
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
- Class: rviz/Time
|
||||
Experimental: false
|
||||
Name: Time
|
||||
SyncMode: 0
|
||||
SyncSource: Image
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.0299999993
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Alpha: 1
|
||||
Class: rviz/RobotModel
|
||||
Collision Enabled: false
|
||||
Enabled: true
|
||||
Links:
|
||||
All Links Enabled: true
|
||||
Expand Joint Details: false
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
body_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
cam_gazebo_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
cam_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
head_pan_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
head_tilt_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_ank_pitch_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_ank_roll_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_el_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_hip_pitch_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_hip_roll_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_hip_yaw_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_knee_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_sho_pitch_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
l_sho_roll_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_ank_pitch_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_ank_roll_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_el_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_hip_pitch_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_hip_roll_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_hip_yaw_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_knee_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_sho_pitch_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
r_sho_roll_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
Name: RobotModel
|
||||
Robot Description: robot_description
|
||||
TF Prefix: ""
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
Visual Enabled: true
|
||||
- Class: rviz/Image
|
||||
Enabled: true
|
||||
Image Topic: /usb_cam_node/image_raw
|
||||
Max Value: 1
|
||||
Median window: 5
|
||||
Min Value: 0
|
||||
Name: Image
|
||||
Normalize Range: true
|
||||
Queue Size: 2
|
||||
Transport Hint: raw
|
||||
Unreliable: false
|
||||
Value: true
|
||||
- Class: rviz/TF
|
||||
Enabled: true
|
||||
Frame Timeout: 15
|
||||
Frames:
|
||||
All Enabled: true
|
||||
body_link:
|
||||
Value: true
|
||||
cam_gazebo_link:
|
||||
Value: true
|
||||
cam_link:
|
||||
Value: true
|
||||
head_pan_link:
|
||||
Value: true
|
||||
head_tilt_link:
|
||||
Value: true
|
||||
l_ank_pitch_link:
|
||||
Value: true
|
||||
l_ank_roll_link:
|
||||
Value: true
|
||||
l_el_link:
|
||||
Value: true
|
||||
l_hip_pitch_link:
|
||||
Value: true
|
||||
l_hip_roll_link:
|
||||
Value: true
|
||||
l_hip_yaw_link:
|
||||
Value: true
|
||||
l_knee_link:
|
||||
Value: true
|
||||
l_sho_pitch_link:
|
||||
Value: true
|
||||
l_sho_roll_link:
|
||||
Value: true
|
||||
r_ank_pitch_link:
|
||||
Value: true
|
||||
r_ank_roll_link:
|
||||
Value: true
|
||||
r_el_link:
|
||||
Value: true
|
||||
r_hip_pitch_link:
|
||||
Value: true
|
||||
r_hip_roll_link:
|
||||
Value: true
|
||||
r_hip_yaw_link:
|
||||
Value: true
|
||||
r_knee_link:
|
||||
Value: true
|
||||
r_sho_pitch_link:
|
||||
Value: true
|
||||
r_sho_roll_link:
|
||||
Value: true
|
||||
world:
|
||||
Value: true
|
||||
Marker Scale: 0.200000003
|
||||
Name: TF
|
||||
Show Arrows: false
|
||||
Show Axes: true
|
||||
Show Names: false
|
||||
Tree:
|
||||
world:
|
||||
body_link:
|
||||
head_pan_link:
|
||||
head_tilt_link:
|
||||
cam_gazebo_link:
|
||||
{}
|
||||
cam_link:
|
||||
{}
|
||||
l_hip_yaw_link:
|
||||
l_hip_roll_link:
|
||||
l_hip_pitch_link:
|
||||
l_knee_link:
|
||||
l_ank_pitch_link:
|
||||
l_ank_roll_link:
|
||||
{}
|
||||
l_sho_pitch_link:
|
||||
l_sho_roll_link:
|
||||
l_el_link:
|
||||
{}
|
||||
r_hip_yaw_link:
|
||||
r_hip_roll_link:
|
||||
r_hip_pitch_link:
|
||||
r_knee_link:
|
||||
r_ank_pitch_link:
|
||||
r_ank_roll_link:
|
||||
{}
|
||||
r_sho_pitch_link:
|
||||
r_sho_roll_link:
|
||||
r_el_link:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: true
|
||||
- Alpha: 0.200000003
|
||||
Class: rviz_plugin_tutorials/Imu
|
||||
Color: 204; 51; 204
|
||||
Enabled: true
|
||||
History Length: 1
|
||||
Name: Imu
|
||||
Topic: /robotis/open_cr/imu
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Default Light: true
|
||||
Fixed Frame: body_link
|
||||
Frame Rate: 30
|
||||
Name: root
|
||||
Tools:
|
||||
- Class: rviz/Interact
|
||||
Hide Inactive Objects: true
|
||||
- Class: rviz/MoveCamera
|
||||
- Class: rviz/Select
|
||||
- Class: rviz/FocusCamera
|
||||
- Class: rviz/Measure
|
||||
- Class: rviz/SetInitialPose
|
||||
Topic: /initialpose
|
||||
- Class: rviz/SetGoal
|
||||
Topic: /move_base_simple/goal
|
||||
- Class: rviz/PublishPoint
|
||||
Single click: true
|
||||
Topic: /clicked_point
|
||||
Value: true
|
||||
Views:
|
||||
Current:
|
||||
Class: rviz/Orbit
|
||||
Distance: 2.32116389
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.0599999987
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Focal Point:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.0500000007
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.00999999978
|
||||
Pitch: 0.275397927
|
||||
Target Frame: <Fixed Frame>
|
||||
Value: Orbit (rviz)
|
||||
Yaw: 5.68858385
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 1023
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: true
|
||||
Image:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd00000004000000000000023300000358fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006600fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000001f1000000df00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000023a000001610000001800ffffff000000010000010f00000358fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000358000000b800fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000022400fffffffb0000000800540069006d00650100000000000004500000000000000000000005470000035800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Selection:
|
||||
collapsed: false
|
||||
Time:
|
||||
collapsed: false
|
||||
Tool Properties:
|
||||
collapsed: false
|
||||
Views:
|
||||
collapsed: true
|
||||
Width: 1920
|
||||
X: 0
|
||||
Y: 0
|
@ -18,7 +18,7 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
op3_action_module_msgs
|
||||
cmake_modules
|
||||
robotis_math
|
||||
ball_detector
|
||||
op3_ball_detector
|
||||
)
|
||||
|
||||
find_package(Boost REQUIRED COMPONENTS thread)
|
||||
@ -71,7 +71,7 @@ catkin_package(
|
||||
op3_action_module_msgs
|
||||
cmake_modules
|
||||
robotis_math
|
||||
ball_detector
|
||||
op3_ball_detector
|
||||
DEPENDS Boost EIGEN3
|
||||
)
|
||||
|
||||
|
@ -29,7 +29,7 @@
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include "robotis_controller_msgs/JointCtrlModule.h"
|
||||
#include "ball_detector/CircleSetStamped.h"
|
||||
#include "op3_ball_detector/CircleSetStamped.h"
|
||||
#include "op3_walking_module_msgs/WalkingParam.h"
|
||||
#include "op3_walking_module_msgs/GetWalkingParam.h"
|
||||
|
||||
|
@ -29,7 +29,7 @@
|
||||
#include <yaml-cpp/yaml.h>
|
||||
|
||||
#include "robotis_controller_msgs/JointCtrlModule.h"
|
||||
#include "ball_detector/CircleSetStamped.h"
|
||||
#include "op3_ball_detector/CircleSetStamped.h"
|
||||
#include "op3_walking_module_msgs/WalkingParam.h"
|
||||
#include "op3_walking_module_msgs/GetWalkingParam.h"
|
||||
|
||||
@ -79,7 +79,7 @@ protected:
|
||||
const int WAITING_THRESHOLD;
|
||||
const bool DEBUG_PRINT;
|
||||
|
||||
void ballPositionCallback(const 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();
|
||||
|
@ -4,7 +4,7 @@
|
||||
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
|
||||
|
||||
<!-- Camera and Ball detector -->
|
||||
<include file="$(find 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" />
|
||||
|
@ -5,7 +5,7 @@
|
||||
<include file="$(find op3_manager)/launch/op3_manager.launch"/>
|
||||
|
||||
<!-- Camera and Ball detector -->
|
||||
<include file="$(find 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" />
|
||||
|
@ -24,7 +24,7 @@
|
||||
<depend>op3_action_module_msgs</depend>
|
||||
<depend>cmake_modules</depend>
|
||||
<depend>robotis_math</depend>
|
||||
<depend>ball_detector</depend>
|
||||
<depend>op3_ball_detector</depend>
|
||||
<depend>boost</depend>
|
||||
<depend>eigen</depend>
|
||||
<depend>yaml-cpp</depend>
|
||||
|
@ -59,7 +59,7 @@ BallTracker::~BallTracker()
|
||||
|
||||
}
|
||||
|
||||
void BallTracker::ballPositionCallback(const ball_detector::CircleSetStamped::ConstPtr &msg)
|
||||
void BallTracker::ballPositionCallback(const op3_ball_detector::CircleSetStamped::ConstPtr &msg)
|
||||
{
|
||||
for (int idx = 0; idx < msg->circles.size(); idx++)
|
||||
{
|
||||
|
@ -13,7 +13,7 @@
|
||||
<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>
|
||||
<exec_depend>ball_detector</exec_depend>
|
||||
<exec_depend>op3_ball_detector</exec_depend>
|
||||
<exec_depend>op3_demo</exec_depend>
|
||||
<export><metapackage/></export>
|
||||
</package>
|
||||
|
Loading…
Reference in New Issue
Block a user