diff --git a/motoman_calib/CMakeLists.txt b/motoman_calib/CMakeLists.txt new file mode 100644 index 0000000..d441b04 --- /dev/null +++ b/motoman_calib/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 2.8.3) +project(motoman_calib) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + sensor_msgs + pcl_ros + pcl_conversions + tf_conversions + ) + +find_package(VTK REQUIRED) + +catkin_package( + CATKIN_DEPENDS +) + +include_directories( + include +${catkin_INCLUDE_DIRS}) + +## Declare a C++ executable +##add_executable(motoman_calib src/calib.cpp) +add_executable(motoman_calib src/calib_yuji.cpp) + + + +## Specify libraries to link a library or executable target against +target_link_libraries(motoman_calib pcl_common pcl_io + ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${VTK_LIBRARIES} +) + diff --git a/motoman_calib/config/calib.rviz b/motoman_calib/config/calib.rviz new file mode 100644 index 0000000..57a0622 --- /dev/null +++ b/motoman_calib/config/calib.rviz @@ -0,0 +1,428 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /TF1/Frames1 + - /PointCloud22 + - /PointCloud23 + - /PointCloud24 + Splitter Ratio: 0.5 + Tree Height: 747 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 0.5 + 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 + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_adapter_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_base_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_base_middle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_base_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_middle_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_middle_middle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_middle_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_top_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_top_middle_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_finger_top_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dhand_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + kinect_center_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + kinect_center_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + kinect_hand_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + kinect_hand_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + kinect_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + kinect_left_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + kinect_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + kinect_right_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + link_b: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_e: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_l: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_r: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_s: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_t: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_u: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool0: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: false + Marker Scale: 0.5 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 1.42972 + Min Value: -5.8683 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.0025 + Style: Spheres + Topic: /kinect_first/qhd/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: jsk_rviz_plugin/BoundingBoxArray + Enabled: false + Name: BoundingBoxArray + Topic: /clustering_result + Unreliable: false + Value: false + alpha: 0.8 + color: 25; 255; 0 + coloring: Auto + line width: 0.005 + only edge: false + show coords: false + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: false + Show Descriptions: true + Show Visual Aids: false + Update Topic: /posestamped_im/update + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 85; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.005 + Style: Flat Squares + Topic: /shifted_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.897967 + Min Value: 0.396679 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.005 + Style: Flat Squares + Topic: /corrected_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.05 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 85; 170; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.005 + Style: Flat Squares + Topic: /mesh_cloud + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + 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: 3.04225 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.350749 + Y: 0.0909696 + Z: 0.455528 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.749796 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.0519 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1028 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000016b0000037afc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000037a000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002f4000000ca0000000000000000fb0000000a0049006d00610067006502000009560000007c0000047b00000309fb0000000c00430061006d00650072006100000002390000009a0000000000000000fb0000000a0049006d00610067006500000002ad000000f50000000000000000000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003c00000003efc0100000002fb0000000800540069006d00650100000000000003c0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000024f0000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 960 + X: 2870 + Y: 24 diff --git a/motoman_calib/launch/calib_gazebo.launch b/motoman_calib/launch/calib_gazebo.launch new file mode 100644 index 0000000..dae2d68 --- /dev/null +++ b/motoman_calib/launch/calib_gazebo.launch @@ -0,0 +1,4 @@ + + + + diff --git a/motoman_calib/package.xml b/motoman_calib/package.xml new file mode 100644 index 0000000..11ed900 --- /dev/null +++ b/motoman_calib/package.xml @@ -0,0 +1,30 @@ + + + motoman_calib + 0.0.0 + The motoman_calib package + + catkin + + Yuji-Ninomiya + Ariyu + BSD + http://github.com/Nishida-Lab/motoman_project + + catkin + roscpp + rospy + sensor_msgs + pcl_ros + pcl_conversions + tf_conversions + roscpp + rospy + sensor_msgs + pcl_ros + pcl_conversions + tf_conversions + tf + tf2 + + diff --git a/motoman_calib/src/calib.cpp b/motoman_calib/src/calib.cpp new file mode 100644 index 0000000..e69de29 diff --git a/motoman_calib/src/calib_yuji.cpp b/motoman_calib/src/calib_yuji.cpp new file mode 100644 index 0000000..46cbe41 --- /dev/null +++ b/motoman_calib/src/calib_yuji.cpp @@ -0,0 +1,481 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +const int sampling_points = 10000; + +// #define gazebo + +inline double +uniform_deviate (int seed) +{ + double ran = seed * (1.0 / (RAND_MAX + 1.0)); + return ran; +} + +inline void +randomPointTriangle(float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3, + Eigen::Vector4f& p) +{ + float r1 = static_cast (uniform_deviate (rand ())); + float r2 = static_cast (uniform_deviate (rand ())); + float r1sqr = sqrtf (r1); + float OneMinR1Sqr = (1 - r1sqr); + float OneMinR2 = (1 - r2); + a1 *= OneMinR1Sqr; + a2 *= OneMinR1Sqr; + a3 *= OneMinR1Sqr; + b1 *= OneMinR2; + b2 *= OneMinR2; + b3 *= OneMinR2; + c1 = r1sqr * (r2 * c1 + b1) + a1; + c2 = r1sqr * (r2 * c2 + b2) + a2; + c3 = r1sqr * (r2 * c3 + b3) + a3; + p[0] = c1; + p[1] = c2; + p[2] = c3; + p[3] = 0; +} + +inline void +randPSurface (vtkPolyData * polydata, std::vector * cumulativeAreas, double totalArea, Eigen::Vector4f& p) +{ + float r = static_cast (uniform_deviate (rand ()) * totalArea); + + std::vector::iterator low = std::lower_bound (cumulativeAreas->begin (), cumulativeAreas->end (), r); + vtkIdType el = vtkIdType (low - cumulativeAreas->begin ()); + + double A[3], B[3], C[3]; + vtkIdType npts = 0; + vtkIdType *ptIds = NULL; + polydata->GetCellPoints (el, npts, ptIds); + polydata->GetPoint (ptIds[0], A); + polydata->GetPoint (ptIds[1], B); + polydata->GetPoint (ptIds[2], C); + randomPointTriangle (float (A[0]), float (A[1]), float (A[2]), + float (B[0]), float (B[1]), float (B[2]), + float (C[0]), float (C[1]), float (C[2]), p); +} + +void +uniform_sampling (vtkSmartPointer polydata, size_t n_samples, pcl::PointCloud & cloud_out) +{ + polydata->BuildCells (); + vtkSmartPointer cells = polydata->GetPolys (); + + double p1[3], p2[3], p3[3], totalArea = 0; + std::vector cumulativeAreas (cells->GetNumberOfCells (), 0); + size_t i = 0; + vtkIdType npts = 0, *ptIds = NULL; + for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds); i++) + { + polydata->GetPoint (ptIds[0], p1); + polydata->GetPoint (ptIds[1], p2); + polydata->GetPoint (ptIds[2], p3); + totalArea += vtkTriangle::TriangleArea (p1, p2, p3); + cumulativeAreas[i] = totalArea; + } + + cloud_out.points.resize (n_samples); + cloud_out.width = static_cast (n_samples); + cloud_out.height = 1; + + for (i = 0; i < n_samples; i++) + { + Eigen::Vector4f p; + randPSurface (polydata, &cumulativeAreas, totalArea, p); + cloud_out.points[i].x = p[0]; + cloud_out.points[i].y = p[1]; + cloud_out.points[i].z = p[2]; + } +} + +class MotomanMeshCloud +{ +public: + MotomanMeshCloud() + : rate_(1), pcl_shifted_cloud_(new pcl::PointCloud()), init_icp_finished_(false) + { + link_names_.push_back("base_link.stl"); + frame_names_.push_back("/base_link"); + link_names_.push_back("link_s.stl"); + frame_names_.push_back("/link_s"); + link_names_.push_back("link_l.stl"); + frame_names_.push_back("/link_l"); + link_names_.push_back("link_e.stl"); + frame_names_.push_back("/link_e"); + link_names_.push_back("link_u.stl"); + frame_names_.push_back("/link_u"); + link_names_.push_back("link_r.stl"); + frame_names_.push_back("/link_r"); + link_names_.push_back("link_b.stl"); + frame_names_.push_back("/link_b"); + link_names_.push_back("link_t.stl"); + frame_names_.push_back("/link_t"); + + corrected_cloud_frame_ = "kinect2_rgb_optical_frame"; + + for(int i = 0; i < link_names_.size(); ++i){ + this->getMesh(ros::package::getPath("motoman_description")+"/meshes/sia5/collision/STL/"+link_names_[i], frame_names_[i]); + } + + frame_names_.push_back("/kinect_hand_link"); + this->getMesh(ros::package::getPath("motoman_description")+"/meshes/sensor/visual/COLLADA/kinectv2.stl", "kinect_hand_link"); + + frame_names_.push_back("dhand_adapter_link"); + frame_names_.push_back("dhand_base_link"); + frame_names_.push_back("dhand_finger_base_left_link"); + frame_names_.push_back("dhand_finger_base_middle_link"); + frame_names_.push_back("dhand_finger_base_right_link"); + frame_names_.push_back("dhand_finger_middle_left_link"); + frame_names_.push_back("dhand_finger_middle_middle_link"); + frame_names_.push_back("dhand_finger_middle_right_link"); + frame_names_.push_back("dhand_finger_top_left_link"); + frame_names_.push_back("dhand_finger_top_middle_link"); + frame_names_.push_back("dhand_finger_top_right_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/adapter.STL", "dhand_adapter_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/base.STL", "dhand_base_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_base.STL", "dhand_finger_base_left_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_base.STL", "dhand_finger_base_middle_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_base.STL", "dhand_finger_base_right_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_middle.STL", "dhand_finger_middle_left_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_middle.STL", "dhand_finger_middle_middle_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_middle.STL", "dhand_finger_middle_right_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_top.STL", "dhand_finger_top_left_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_top.STL", "dhand_finger_top_middle_link"); + this->getMesh(ros::package::getPath("dhand_description")+"/meshes/collision/finger/finger_top.STL", "dhand_finger_top_right_link"); + + + this->transformMesh(); + // ### Node Handles ### + mesh_pointcloud_publisher_ = nh_.advertise("/mesh_cloud", 1); + shifted_cloud_publisher_ = nh_.advertise("/shifted_cloud",1); + corrected_cloud_publisher_ = nh_.advertise("/corrected_cloud",1); + //kinect_subscriber_ = nh_.subscribe("/kinect_first/hd/points", 10, boost::bind(&MotomanMeshCloud::pointCloudCallback, this, _1)); + kinect_subscriber_ = nh_.subscribe("/kinect_left/hd/points", 10, boost::bind(&MotomanMeshCloud::pointCloudCallback, this, _1)); + frame_timer_ = nh_.createTimer(ros::Duration(0.01), boost::bind(&MotomanMeshCloud::frameCallback, this, _1)); + } + ~MotomanMeshCloud() + { + } + void getMesh(std::string dir_path, std::string frame_name) + { + pcl::PolygonMesh mesh; + pcl::io::loadPolygonFileSTL(dir_path, mesh); + vtkSmartPointer polydata1 = vtkSmartPointer::New();; + pcl::io::mesh2vtk(mesh, polydata1); + pcl::PointCloud::Ptr parts_cloud(new pcl::PointCloud); + uniform_sampling (polydata1, sampling_points, *parts_cloud); + parts_clouds_.push_back(*parts_cloud); + + //pcl_conversions::fromPCL(*cloud_1, mesh_pointcloud_); + } + + void transformMesh() + { + pcl::PointCloud transformed_parts_cloud; + for (int retryCount = 0; retryCount < 10; ++retryCount) { + for(size_t i = 0; i< parts_clouds_.size(); ++i){ + try{ + tf::StampedTransform transform; + tf_.lookupTransform("/base_link", frame_names_[i], + ros::Time(0), transform); + pcl_ros::transformPointCloud(parts_clouds_[i],transformed_parts_cloud, transform); + sia5_cloud_ += transformed_parts_cloud; + ROS_INFO_STREAM("Get transform : " << frame_names_[i]); + }catch(tf2::LookupException e) + { + ROS_ERROR("pcl::ros %s",e.what()); + ros::Duration(1.0).sleep(); + sia5_cloud_.clear(); + break; + }catch(tf2::ExtrapolationException e) + { + ROS_ERROR("pcl::ros %s",e.what()); + ros::Duration(1.0).sleep(); + sia5_cloud_.clear(); + break; + }catch(...) + { + ros::Duration(1.0).sleep(); + sia5_cloud_.clear(); + break; + } + } + if(sia5_cloud_.points.size() == (sampling_points*parts_clouds_.size())){ + std::cout << "0) mesh : " << sia5_cloud_.points.size() << std::endl; + return; + } + } + ROS_WARN_STREAM("try tf transform 5times, but failed"); + exit(-1); + return; + } + + void cropBox(pcl::PointCloud::Ptr cloud, + pcl::PointCloud::Ptr cloud_filtered) + { + pcl::PassThrough passthrough_filter; + passthrough_filter.setInputCloud(cloud); + passthrough_filter.setFilterFieldName("z"); + // passthrough_filter.setFilterLimits(-1.0, 0.1); // init for left + passthrough_filter.setFilterLimits(0.1, 1.0); //other position + passthrough_filter.setFilterLimitsNegative (false); + passthrough_filter.filter (*cloud); + passthrough_filter.setInputCloud(cloud); + passthrough_filter.setFilterFieldName("x"); + passthrough_filter.setFilterLimits(-1.0, -0.1); // init for left + // passthrough_filter.setFilterLimits(-0.1, 0.9); //other position + passthrough_filter.setFilterLimitsNegative (true); + passthrough_filter.filter (*cloud); + passthrough_filter.setInputCloud(cloud); + /*passthrough_filter.setFilterFieldName("y"); + // passthrough_filter.setFilterLimits(-1.0, -0.1); // init for left + passthrough_filter.setFilterLimits(0.0, 0.5); //other position + passthrough_filter.setFilterLimitsNegative (false); + passthrough_filter.filter (*cloud);*/ + std::cout << "------------------------------------" << std::endl; + std::cout << "filtered cloud point size : " << cloud->points.size() << std::endl; + + pcl::search::KdTree kdtree; + kdtree.setInputCloud(cloud); + pcl::PointXYZ search_point; + search_point.x = 0; + search_point.y = 0; + search_point.z = 0; + double search_radius = 1.0; + std::vector point_radius_squared_distance; + pcl::PointIndices::Ptr point_idx_radius_search(new pcl::PointIndices()); + + if ( kdtree.radiusSearch (search_point, search_radius, point_idx_radius_search->indices, point_radius_squared_distance) > 0 ) + { + pcl::ExtractIndices extractor; + extractor.setInputCloud(cloud); + extractor.setIndices(point_idx_radius_search); + extractor.setNegative(false); + extractor.filter(*cloud_filtered); + } + std::cout << "Passthrough filtering..." << std::endl; + } + + void downSampling(pcl::PointCloud::Ptr cloud, + pcl::PointCloud::Ptr cloud_filtered) + { + pcl::VoxelGrid sor; + sor.setInputCloud (cloud); + sor.setLeafSize (0.01f, 0.01f, 0.01f); + sor.filter (*cloud_filtered); + std::cout << "Down sampling..." << std::endl; + } + + pcl::PointCloud shiftPointCloud(pcl::PointCloud points, double x, double y, double z, double roll, double pitch, double yaw) + { + tf::Matrix3x3 init_rotation; + Eigen::AngleAxisf rotation_x(roll, Eigen::Vector3f::UnitX()); + Eigen::AngleAxisf rotation_y(pitch, Eigen::Vector3f::UnitY()); + Eigen::AngleAxisf rotation_z(yaw, Eigen::Vector3f::UnitZ()); + Eigen::Translation3f translation(x, y, z); + Eigen::Matrix4f noize_matrix = (translation * rotation_z * rotation_y * rotation_x).matrix(); + pcl::PointCloud shifted_cloud; + pcl::transformPointCloud(points, shifted_cloud, noize_matrix); + return shifted_cloud; + } + + void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& kinect_pointcloud) + { + if(init_icp_finished_){ + return; + }else{ + // ROSのメッセージからPCLの形式に変換 + pcl::PointCloud::Ptr pcl_pc(new pcl::PointCloud()); + pcl::fromROSMsg(*kinect_pointcloud, *pcl_pc); + tf::StampedTransform transform; + // world 座標系に変換 + try{ + tf_.waitForTransform("/base_link", kinect_pointcloud->header.frame_id, ros::Time(0), ros::Duration(10.0)); + tf_.lookupTransform("/base_link", kinect_pointcloud->header.frame_id, ros::Time(0), transform); + Eigen::Matrix4f eigen_transform; + pcl_ros::transformAsMatrix(transform, eigen_transform); + + std::cout << "----Initial position--------------------" << std::endl; + std::cout << eigen_transform << std::endl; + std::cout << "----------------------------------------" << std::endl; + std::cout << "kinect :" << kinect_pointcloud->data.size() << std::endl; + + Eigen::Affine3f eigen_affine_transform(eigen_transform); + pcl::transformPointCloud(*pcl_pc, *pcl_shifted_cloud_, eigen_affine_transform); + + this->cropBox(pcl_shifted_cloud_, pcl_shifted_cloud_); + this->downSampling(pcl_shifted_cloud_, pcl_shifted_cloud_); + + ROS_INFO_STREAM("shifted_cloud size : " << pcl_shifted_cloud_->points.size()); + sensor_msgs::PointCloud2 ros_shifted_cloud; + pcl::toROSMsg(*pcl_shifted_cloud_, ros_shifted_cloud); //pclで処理したやつをros形式へ変換 + ros_shifted_cloud.header.stamp = ros::Time::now(); + ros_shifted_cloud.header.frame_id = "/base_link"; + corrected_cloud_frame_ = "/base_link"; + shifted_cloud_publisher_.publish(ros_shifted_cloud); // デバッグのためにずらしたやつのpublish + + }catch(tf::TransformException ex){ + ROS_ERROR("%s", ex.what()); + } + + } + } + + void frameCallback(const ros::TimerEvent&) + { + std::cout << "frame call back" << std::endl; + ros::Time time = ros::Time::now(); + br_.sendTransform(tf::StampedTransform(fixed_kinect_frame_, time, "world", "fixed_kinect_frame")); // Rvizへの出力? + } + + void run() + { + while(ros::ok()) + { + pcl::toROSMsg(sia5_cloud_, mesh_pointcloud_); + mesh_pointcloud_.header.stamp = ros::Time::now(); + mesh_pointcloud_.header.frame_id = "/base_link"; + mesh_pointcloud_publisher_.publish(mesh_pointcloud_); + // ### ICP Argolithm #### + pcl::IterativeClosestPoint icp; + pcl::PointCloud::Ptr sia5_ptr(new pcl::PointCloud(sia5_cloud_)); + std::vector nan_index; + pcl::removeNaNFromPointCloud(*pcl_shifted_cloud_, *pcl_shifted_cloud_, nan_index); + pcl::removeNaNFromPointCloud(*sia5_ptr, *sia5_ptr, nan_index); + + //icp.setMaximumIterations(1000); + if(pcl_shifted_cloud_->points.size() != 0){ + pcl::PointCloud Final; + Eigen::Vector4f c_sia5, c_kinect; + pcl::compute3DCentroid (*sia5_ptr, c_sia5); // 重心計算して代入 + pcl::compute3DCentroid (*pcl_shifted_cloud_, c_kinect); // フィルタかけた点群の重心計算 + Eigen::AngleAxisf init_rotation (0.0, Eigen::Vector3f::UnitZ ()); + Eigen::Translation3f init_translation (c_sia5(0,0)-c_kinect(0,0), c_sia5(1,0)-c_kinect(1,0), c_sia5(2,0)-c_kinect(2,0)); + Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix (); + ROS_INFO_STREAM("Matching Start!!!"); + icp.setInputSource(pcl_shifted_cloud_); + icp.setInputTarget(sia5_ptr); + icp.setMaximumIterations(1000); + icp.setTransformationEpsilon (1e-13); + icp.setEuclideanFitnessEpsilon (1e-10); + icp.align(Final, init_guess); + ROS_INFO_STREAM("has converged : " << icp.hasConverged()); + ROS_INFO_STREAM("score : " << icp.getFitnessScore()); + /*try{ + tf::StampedTransform transform; // icpかました後のカメラ位置推定処理 + tf_.lookupTransform("/base_link", "kinect_first_link", + ros::Time::now(), transform); + Eigen::Affine3d kinect_to_world_transform; + tf::transformTFToEigen(transform, kinect_to_world_transform); + Eigen::Matrix4d world_to_corrected = (icp.getFinalTransformation()).cast(); //最終的に得られたICPによる変換行列を変数として置く + Eigen::Matrix4d matrix_kinect_to_world = kinect_to_world_transform.matrix(); //kinectからワールド座標系への変換行列の定義 + Eigen::Matrix4d kinect_first_to_corrected = world_to_corrected*matrix_kinect_to_world; //座標変換 + kinect_first_to_corrected = kinect_first_to_corrected.normalized(); //正規化失敗はここ？ + Eigen::Affine3d eigen_affine3d(kinect_first_to_corrected); + tf::transformEigenToTF(eigen_affine3d, fixed_kinect_frame_); + Eigen::Matrix3d rotation_matrix = kinect_first_to_corrected.block(0, 0, 3, 3); //←なにこれ？（笑） + Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); //回転行列からオイラー角を計算 + std::cout << "" << std::endl; + }catch(...){ + ROS_ERROR("tf fail"); + }*/ + try{ + tf::StampedTransform transform; // icpかました後のカメラ位置推定処理 + tf_.lookupTransform("/base_link", "kinect_left_link", + ros::Time::now(), transform); + Eigen::Affine3d kinect_to_world_transform; + tf::transformTFToEigen(transform, kinect_to_world_transform); // Initial position + Eigen::Matrix4d matrix_kinect_to_world = kinect_to_world_transform.matrix(); //kinectからワールド座標系への変換行列の定義 + Eigen::Matrix4d kinect_to_corrected = (icp.getFinalTransformation()).cast(); //最終的に得られたICPによる変換行列を変数として置く + std::cout << "----ICP's matrix------------------------" << std::endl; + std::cout << kinect_to_corrected << std::endl; + std::cout << "----------------------------------------" << std::endl; + /*Eigen::Matrix4d kinect_left_to_corrected = world_to_corrected*matrix_kinect_to_world; //座標変換*/ + Eigen::Matrix4d kinect_left_to_corrected = kinect_to_corrected*matrix_kinect_to_world; + + std::cout << "----Corrected position------------------" << std::endl; + std::cout << kinect_left_to_corrected << std::endl; + std::cout << "----------------------------------------" << std::endl; + + Eigen::Matrix4d kinect_left_corrected = kinect_left_to_corrected.normalized(); //正規化失敗はここ？ + Eigen::Affine3d eigen_affine3d(kinect_left_corrected); + tf::transformEigenToTF(eigen_affine3d, fixed_kinect_frame_); + Eigen::Matrix3d rotation_matrix = kinect_left_to_corrected.block(0, 0, 3, 3); // matrix of 3 times 3 started from (0,0) + Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); //回転行列からオイラー角を計算 + std::cout << "" << std::endl; + }catch(...){ + ROS_ERROR("tf fail"); + } + sensor_msgs::PointCloud2 ros_corrected_cloud; + pcl::toROSMsg(Final, ros_corrected_cloud); //ICPこの関数の最初で定義した点群を新しい文字に代入 + ros_corrected_cloud.header.stamp = ros::Time::now(); + ros_corrected_cloud.header.frame_id = "/base_link"; + corrected_cloud_publisher_.publish(ros_corrected_cloud); //変換した点群をpublish + } + ros::spinOnce(); + rate_.sleep(); + std::cout << "===================================" << std::endl; + } + } +private: + std::vector meshes_; + sensor_msgs::PointCloud2 mesh_pointcloud_; + ros::NodeHandle nh_; + tf::TransformListener tf_; + tf::TransformBroadcaster br_; + ros::Rate rate_; + ros::Publisher mesh_pointcloud_publisher_; + ros::Publisher shifted_cloud_publisher_; + ros::Publisher corrected_cloud_publisher_; + ros::Timer frame_timer_; + ros::Subscriber kinect_subscriber_; + std::vector link_names_; + std::vector frame_names_; + pcl::PointCloud sia5_cloud_; + std::vector >parts_clouds_; + sensor_msgs::PointCloud2 kinect_pointcloud_; + pcl::PointCloud::Ptr pcl_shifted_cloud_; + std::string corrected_cloud_frame_; + bool init_icp_finished_; + tf::Transform fixed_kinect_frame_; +}; + + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "motoman_calib"); + MotomanMeshCloud mesh_cloud; + mesh_cloud.run(); + return 0; +} diff --git a/motoman_control/launch/sia5/sia5_with_dhand_and_3_kinects_streaming.launch b/motoman_control/launch/sia5/sia5_with_dhand_and_3_kinects_streaming.launch new file mode 100644 index 0000000..630b266 --- /dev/null +++ b/motoman_control/launch/sia5/sia5_with_dhand_and_3_kinects_streaming.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + [joint_s, joint_l, joint_e, joint_u, joint_r, joint_b, joint_t] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motoman_demo/launch/handring_parallel2.launch b/motoman_demo/launch/handring_parallel2.launch new file mode 100644 index 0000000..2f470af --- /dev/null +++ b/motoman_demo/launch/handring_parallel2.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/motoman_demo/launch/handring_parallel2_trajectory.launch b/motoman_demo/launch/handring_parallel2_trajectory.launch new file mode 100644 index 0000000..1369032 --- /dev/null +++ b/motoman_demo/launch/handring_parallel2_trajectory.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/motoman_demo/launch/handring_parallel_attached_object.launch b/motoman_demo/launch/handring_parallel_attached_object.launch new file mode 100644 index 0000000..bbc4f1a --- /dev/null +++ b/motoman_demo/launch/handring_parallel_attached_object.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/motoman_demo/launch/handring_parallel_for_pick_and_place.launch b/motoman_demo/launch/handring_parallel_for_pick_and_place.launch new file mode 100644 index 0000000..2251742 --- /dev/null +++ b/motoman_demo/launch/handring_parallel_for_pick_and_place.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/motoman_demo/launch/handring_parallel_for_pick_and_place2.launch b/motoman_demo/launch/handring_parallel_for_pick_and_place2.launch new file mode 100644 index 0000000..811638b --- /dev/null +++ b/motoman_demo/launch/handring_parallel_for_pick_and_place2.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/motoman_demo/launch/handring_parallel_sidebox.launch b/motoman_demo/launch/handring_parallel_sidebox.launch new file mode 100644 index 0000000..8773a00 --- /dev/null +++ b/motoman_demo/launch/handring_parallel_sidebox.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/motoman_demo/scripts/.#handring_parallel_planner_sidebox.py b/motoman_demo/scripts/.#handring_parallel_planner_sidebox.py new file mode 120000 index 0000000..2568ede --- /dev/null +++ b/motoman_demo/scripts/.#handring_parallel_planner_sidebox.py @@ -0,0 +1 @@ +motoman@nishida-lab.29490:1520230262 \ No newline at end of file diff --git a/motoman_demo/scripts/handring_parallel_planner.py b/motoman_demo/scripts/handring_parallel_planner.py index f64d30d..87600d2 100755 --- a/motoman_demo/scripts/handring_parallel_planner.py +++ b/motoman_demo/scripts/handring_parallel_planner.py @@ -26,8 +26,8 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Header # for object bounding box -from jsk_recognition_msgs.msg import BoundingBoxArray -from jsk_recognition_msgs.msg import BoundingBox +from motoman_viz_msgs.msg import BoundingBoxArray +from motoman_viz_msgs.msg import BoundingBox # == Service == # for cleaning the Octomap from std_srvs.srv import Empty diff --git a/motoman_demo/scripts/handring_parallel_planner2.py b/motoman_demo/scripts/handring_parallel_planner2.py new file mode 100755 index 0000000..b5eca08 --- /dev/null +++ b/motoman_demo/scripts/handring_parallel_planner2.py @@ -0,0 +1,394 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Basic +import sys +import copy +from math import * +# ROS +import rospy +import rosparam +# Moveit +import moveit_commander +# TF +import tf2_ros +import tf +# == Messages == +# for Cartesian path +from geometry_msgs.msg import Pose +# for Pepper message +from std_msgs.msg import String +# for Start state +from moveit_msgs.msg import RobotState +from moveit_msgs.msg import RobotTrajectory +# for Planned path +from motoman_demo_msgs.msg import HandringPlan +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +# for object bounding box +from motoman_viz_msgs.msg import BoundingBoxArray +from motoman_viz_msgs.msg import BoundingBox +# == Service == +# for cleaning the Octomap +from std_srvs.srv import Empty + +class HandringPlanner(object): + + def __init__(self): + # ========= Subscriber ======== # + # self.speech_sub_topic = rospy.get_param('~speech') + self.speech_sub = rospy.Subscriber('/speech', String, self.speechCallback) + + # ========== Moveit init ========== # + # moveit_commander init + self.robot = moveit_commander.RobotCommander() + self.arm = moveit_commander.MoveGroupCommander("arm") + self.target_pose = Pose() + # Set the planning time + self.arm.set_planner_id('RRTConnectkConfigDefault') + self.planning_limitation_time = 5.0 + self.arm.set_planning_time(self.planning_limitation_time) + + # ========== TF ======== # + # TF Listner # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listner = tf2_ros.TransformListener(self.tf_buffer) + + # ========== Handring Plan publisher ======== # + self.hp_pub = rospy.Publisher('~handring_plan', HandringPlan, queue_size=6) + + # ========= Box Poses ======== # + self.box_pose = [{}, {}] + # Box 0 pose + self.box_pose[0]["joint_s"] = 0.33417996764183044 + self.box_pose[0]["joint_l"] = 0.6119770288467407 + self.box_pose[0]["joint_e"] = 0.0 + self.box_pose[0]["joint_u"] = 0.32670751214027405 + self.box_pose[0]["joint_r"] = 0.0 + self.box_pose[0]["joint_b"] = -0.6 + self.box_pose[0]["joint_t"] = 0.0 + # Box 1 pose + self.box_pose[1]["joint_s"] = 0.6041074967384338 + self.box_pose[1]["joint_l"] = 0.5769590139389038 + self.box_pose[1]["joint_e"] = -1.396013617515564 + self.box_pose[1]["joint_u"] = -0.6198193430900574 + self.box_pose[1]["joint_r"] = 0.0 + self.box_pose[1]["joint_b"] = -0.4 + self.box_pose[1]["joint_t"] = 0.0 + + # ======== Object Info ======== # + self.diff = rospy.get_param('~diff_from_object', 0.03) # diff from offset to grasp the object + self.offset = rospy.get_param('~offset', 0.45) # offset from top of the object + self.box_sub = rospy.Subscriber('/clustering_result', BoundingBoxArray, self.bbArrayCallback) + self.initial_box_num = 0 + + rospy.loginfo("HPP Initialized") + + # -------- Bounding Box Array call back -------- # + def bbArrayCallback(self, message): + self.initial_box_num = len(message.boxes) + + # -------- Get TF -------- # + def get_object_tf_data(self, num): + tf_time = rospy.Time(0) + target = "object_" + str(num) + get_tf_flg = False + while not get_tf_flg : + try : + trans = self.tf_buffer.lookup_transform('world', target, tf_time, rospy.Duration(10)) + get_tf_flg = True + + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) : + continue + return trans + + def get_goal_tf_data(self, object_trans, offset_x, offset_y): + goal_trans = copy.deepcopy(object_trans) + goal_trans.transform.translation.x = object_trans.transform.translation.x + offset_x + goal_trans.transform.translation.y = object_trans.transform.translation.y + offset_y + return goal_trans + + # -------- Clear Octomap -------- # + def clear_octomap(self): + rospy.wait_for_service('clear_octomap') + try: + result = rospy.ServiceProxy('clear_octomap', Empty) + result() + except rospy.ServiceException, e: + rospy.logwarn("Couldn't Clear Octomap") + + # -------- Plannning & Execution -------- # + def get_plan(self, trans, z_offset, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + z_offset + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + self.arm.set_pose_target(self.target_pose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + print("---------debug--------{}".format(len(plan.joint_trajectory.points))) + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + def get_cartesian_plan(self, trans, z_offset, start_state, grasp): + # set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # set waypoints + waypoints = [] + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + wpose = Pose() + wpose.position = copy.deepcopy(self.target_pose.position) + wpose.orientation = copy.deepcopy(self.target_pose.orientation) + wpose.position.z = copy.deepcopy(self.target_pose.position.z + z_offset) + waypoints.append(wpose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + (plan, fraction) = self.arm.compute_cartesian_path( + waypoints, # waypoints to follow + 0.01, # eef_step + 0.0) # jump_threshold + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a cartesian plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + + # -------- Go to Home Position -------- # + def get_home_plan(self, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + init_pose = self.arm.get_current_joint_values() + init_pose[0] = 0.0 + init_pose[1] = 0.0 + init_pose[2] = 0.0 + init_pose[3] = 0.0 + init_pose[4] = 0.0 + init_pose[5] = 0.0 + init_pose[6] = 0.0 + self.arm.set_joint_value_target(init_pose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a home plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + # -------- Go to Box Position -------- # + def get_box_plan(self, num, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.arm.set_joint_value_target(self.box_pose[num]) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + rospy.loginfo("!! Got a box plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state, plan) + + # def set_plan(self, plan, grasp): + + # -------- Get message from pepper -------- # + def speechCallback(self, message): + rospy.loginfo("(-O-) Task start (-O-)") + # initialize + start_state = JointState() + start_state.header = Header() + start_state.header.stamp = rospy.Time.now() + start_state.name = rosparam.get_param("/controller_joint_names") + # start_state.name = rosparam.get_param("/sia5_controller/joints") + for i in range(len(start_state.name)): + start_state.position.append(0.) + get_num_from_pepper = int(message.data) + + # do the planning depending on the order number + if get_num_from_pepper == 99 : + rospy.loginfo("Called order 99") + rospy.loginfo("Currently this order is not working..") + object_trans = [] + goal_trans = [] + box_num = self.initial_box_num + rospy.loginfo("%d objects detected...", box_num) + for x in xrange(1, box_num+1): + trans = self.get_object_tf_data(x) + object_trans.append(trans) + goal_trans.append(self.get_goal_tf_data(trans, 0, 0.2)) #offset x, y + for x in xrange(0, box_num): + state = self.run(1, (x+1)%2, start_state, object_trans[x], goal_trans[x]) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + break + rospy.loginfo("No.%i task finished.", x+1) + start_state = state + rospy.loginfo("(^O^) All task finished (^O^)") + + else : + object_num = get_num_from_pepper / 10 + box_num = get_num_from_pepper % 10 - 1 + object_trans = self.get_object_tf_data(object_num) + goal_trans = self.get_goal_tf_data(object_trans, 0, 0.2) #offset x, y + self.run(object_num, box_num, start_state, object_trans, goal_trans) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + rospy.loginfo("(^O^) All task finished (^O^)") + + # -------- Shutdown -------- # + def shutdown(self): + rospy.logwarn("(xOx) Aborted (xOx)") + + # -------- Run the Program -------- # + def run(self, obj_num, box_num, start_state, object_trans, goal_trans): + + # Go to Grasp + (result, state) = self.get_plan(object_trans, self.offset, start_state, False) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(object_trans, 0.3 + self.diff, state, True) + if rospy.is_shutdown(): + return + # Back to upper side + (result, state) = self.get_cartesian_plan(object_trans, self.offset + 0.05, state, True) + if rospy.is_shutdown(): + return + + # Back to home + (result, state) = self.get_home_plan(state, True) + if rospy.is_shutdown(): + return + + # Go to Release + (result, state) = self.get_plan(goal_trans, self.offset, state, True) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(goal_trans, 0.3 + self.diff, state, False) + if rospy.is_shutdown(): + return + # Back to upper side + (result, state) = self.get_cartesian_plan(goal_trans, self.offset + 0.05, state, False) + if rospy.is_shutdown(): + return + + # Back to home + (result, state) = self.get_home_plan(state, False) + if rospy.is_shutdown(): + return + + return state + + +if __name__ == '__main__': + rospy.init_node("handring_parallel_planner") + handring_planner = HandringPlanner() + rospy.spin() diff --git a/motoman_demo/scripts/handring_parallel_planner2_trajectory.py b/motoman_demo/scripts/handring_parallel_planner2_trajectory.py new file mode 100755 index 0000000..51c2e89 --- /dev/null +++ b/motoman_demo/scripts/handring_parallel_planner2_trajectory.py @@ -0,0 +1,451 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from time import sleep +# Basic +import sys +import copy +from math import * +import numpy as np +# ROS +import rospy +import rosparam +# Moveit +import moveit_commander +# TF +import tf2_ros +import tf +# == Messages == +# for Cartesian path +from geometry_msgs.msg import Pose +# for Pepper message +from std_msgs.msg import String +# for Start state +from moveit_msgs.msg import RobotState +from moveit_msgs.msg import RobotTrajectory +# for Planned path +from motoman_demo_msgs.msg import HandringPlan +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +# for object bounding box +from motoman_viz_msgs.msg import BoundingBoxArray +from motoman_viz_msgs.msg import BoundingBox +# == Service == +# for cleaning the Octomap +from std_srvs.srv import Empty + + +moving_x = 0 +moving_y = 0 + +class HandringPlanner(object): + + def __init__(self): + # ========= Subscriber ======== # + # self.speech_sub_topic = rospy.get_param('~speech') + self.speech_sub = rospy.Subscriber('/speech', String, self.speechCallback) + + # ========== Moveit init ========== # + # moveit_commander init + self.robot = moveit_commander.RobotCommander() + self.arm = moveit_commander.MoveGroupCommander("arm") + self.target_pose = Pose() + # Set the planning time + self.arm.set_planner_id('RRTConnectkConfigDefault') + self.planning_limitation_time = 5.0 + self.arm.set_planning_time(self.planning_limitation_time) + + # ========== TF ======== # + # TF Listner # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listner = tf2_ros.TransformListener(self.tf_buffer) + + # ========== Handring Plan publisher ======== # + self.hp_pub = rospy.Publisher('~handring_plan', HandringPlan, queue_size=6) + + # ========= Box Poses ======== # + self.box_pose = [{}, {}] + # Box 0 pose + self.box_pose[0]["joint_s"] = 0.33417996764183044 + self.box_pose[0]["joint_l"] = 0.6119770288467407 + self.box_pose[0]["joint_e"] = 0.0 + self.box_pose[0]["joint_u"] = 0.32670751214027405 + self.box_pose[0]["joint_r"] = 0.0 + self.box_pose[0]["joint_b"] = -0.6 + self.box_pose[0]["joint_t"] = 0.0 + # Box 1 pose + self.box_pose[1]["joint_s"] = 0.6041074967384338 + self.box_pose[1]["joint_l"] = 0.5769590139389038 + self.box_pose[1]["joint_e"] = -1.396013617515564 + self.box_pose[1]["joint_u"] = -0.6198193430900574 + self.box_pose[1]["joint_r"] = 0.0 + self.box_pose[1]["joint_b"] = -0.4 + self.box_pose[1]["joint_t"] = 0.0 + + # ======== Object Info ======== # + self.diff = rospy.get_param('~diff_from_object', 0.03) # diff from offset to grasp the object + self.offset = rospy.get_param('~offset', 0.45) # offset from top of the object + self.box_sub = rospy.Subscriber('/clustering_result', BoundingBoxArray, self.bbArrayCallback) + self.initial_box_num = 0 + + rospy.loginfo("HPP Initialized") + + # -------- Bounding Box Array call back -------- # + def bbArrayCallback(self, message): + self.initial_box_num = len(message.boxes) + + # -------- Get TF -------- # + def get_object_tf_data(self, num): + tf_time = rospy.Time(0) + target = "object_" + str(num) + get_tf_flg = False + while not get_tf_flg : + try : + trans = self.tf_buffer.lookup_transform('world', target, tf_time, rospy.Duration(10)) + get_tf_flg = True + + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) : + continue + return trans + + def get_goal_tf_data(self, object_trans, offset_x, offset_y): + goal_trans = copy.deepcopy(object_trans) + goal_trans.transform.translation.x = object_trans.transform.translation.x + offset_x + goal_trans.transform.translation.y = object_trans.transform.translation.y + offset_y + return goal_trans + + # -------- Clear Octomap -------- # + def clear_octomap(self): + rospy.wait_for_service('clear_octomap') + try: + result = rospy.ServiceProxy('clear_octomap', Empty) + result() + except rospy.ServiceException, e: + rospy.logwarn("Couldn't Clear Octomap") + + # -------- Plannning & Execution -------- # + def get_plan(self, trans, z_offset, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + z_offset + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + self.arm.set_pose_target(self.target_pose) + # plan + # for i in range(5): + # print "plan: " +str(i) + # plan = RobotTrajectory() + # counter = 0 + # while len(plan.joint_trajectory.points) == 0 : + # plan = self.arm.plan() + # counter+=1 + # self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + # if counter > 1 : + # return (False, start_state) + # self.arm.set_planning_time(self.planning_limitation_time) + # # sleep(1) + # rospy.sleep(0.5) + + #plan + threshold = np.sqrt((moving_x*100)^2 + (moving_y*100)^2) + threshold = 20 + + while(1): + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + if grasp == 0: + break + if len(plan.joint_trajectory.points) < threshold: + # print("---------debug0--------{}".format(len(plan.joint_trajectory.points))) + break + # print "input key A to continue." + # key = raw_input('>>> ') + # if key == "a": + # break + + # plan = RobotTrajectory() + # counter = 0 + # print("---------debug1--------{}".format(len(plan.joint_trajectory.points))) + # while len(plan.joint_trajectory.points) == 0 : + # plan = self.arm.plan() + # counter+=1 + # self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + # if counter > 1 : + # return (False, start_state) + # self.arm.set_planning_time(self.planning_limitation_time) + # print("------------debug------------{}".format(len(plan.joint_trajectory.points))) + + rospy.loginfo("!! Got a plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + # print("---------debug2--------{}".format(len(plan.joint_trajectory.points))) + pub_msg.trajectory = plan + + # print "input key A to continue." + # while(1): + # key = raw_input('>>> ') + # if key == "a": + # break + + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + def get_cartesian_plan(self, trans, z_offset, start_state, grasp): + # set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # set waypoints + waypoints = [] + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + wpose = Pose() + wpose.position = copy.deepcopy(self.target_pose.position) + wpose.orientation = copy.deepcopy(self.target_pose.orientation) + wpose.position.z = copy.deepcopy(self.target_pose.position.z + z_offset) + waypoints.append(wpose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + (plan, fraction) = self.arm.compute_cartesian_path( + waypoints, # waypoints to follow + 0.01, # eef_step + 0.0) # jump_threshold + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a cartesian plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + + # -------- Go to Home Position -------- # + def get_home_plan(self, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + init_pose = self.arm.get_current_joint_values() + init_pose[0] = 0.0 + init_pose[1] = 0.0 + init_pose[2] = 0.0 + init_pose[3] = 0.0 + init_pose[4] = 0.0 + init_pose[5] = 0.0 + init_pose[6] = 0.0 + self.arm.set_joint_value_target(init_pose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a home plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + # -------- Go to Box Position -------- # + def get_box_plan(self, num, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.arm.set_joint_value_target(self.box_pose[num]) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + rospy.loginfo("!! Got a box plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state, plan) + + # def set_plan(self, plan, grasp): + + # -------- Get message from pepper -------- # + def speechCallback(self, message): + rospy.loginfo("(-O-) Task start (-O-)") + # initialize + start_state = JointState() + start_state.header = Header() + start_state.header.stamp = rospy.Time.now() + start_state.name = rosparam.get_param("/controller_joint_names") + # start_state.name = rosparam.get_param("/sia5_controller/joints") + for i in range(len(start_state.name)): + start_state.position.append(0.) + get_num_from_pepper = int(message.data) + + # do the planning depending on the order number + if get_num_from_pepper == 99 : + rospy.loginfo("Called order 99") + rospy.loginfo("Currently this order is not working..") + object_trans = [] + goal_trans = [] + box_num = self.initial_box_num + rospy.loginfo("%d objects detected...", box_num) + for x in xrange(1, box_num+1): + trans = self.get_object_tf_data(x) + object_trans.append(trans) + goal_trans.append(self.get_goal_tf_data(trans, 0, 0.2)) #offset x, y + for x in xrange(0, box_num): + state = self.run(1, (x+1)%2, start_state, object_trans[x], goal_trans[x]) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + break + rospy.loginfo("No.%i task finished.", x+1) + start_state = state + rospy.loginfo("(^O^) All task finished (^O^)") + + else : + object_num = get_num_from_pepper / 10 + box_num = get_num_from_pepper % 10 - 1 + object_trans = self.get_object_tf_data(object_num) + moving_x = float(raw_input('axis:↓ offset_x[m] >>> ')) + moving_y = float(raw_input('axis:→ offset_y[m] >>> ')) + goal_trans = self.get_goal_tf_data(object_trans, moving_x, moving_y) #offset x, y + self.run(object_num, box_num, start_state, object_trans, goal_trans) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + rospy.loginfo("(^O^) All task finished (^O^)") + + # -------- Shutdown -------- # + def shutdown(self): + rospy.logwarn("(xOx) Aborted (xOx)") + + # -------- Run the Program -------- # + def run(self, obj_num, box_num, start_state, object_trans, goal_trans): + + # Go to Grasp + (result, state) = self.get_plan(object_trans, self.offset, start_state, False) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(object_trans, 0.3 + self.diff, state, True) + if rospy.is_shutdown(): + return + # Back to upper side + (result, state) = self.get_cartesian_plan(object_trans, self.offset + 0.05, state, True) + if rospy.is_shutdown(): + return + + # # Back to home + # (result, state) = self.get_home_plan(state, False) + # if rospy.is_shutdown(): + # return + + # Go to Release + (result, state) = self.get_plan(goal_trans, self.offset, state, True) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(goal_trans, 0.3 + self.diff, state, False) + if rospy.is_shutdown(): + return + + + # Back to upper side + (result, state) = self.get_cartesian_plan(goal_trans, self.offset + 0.05, state, False) + if rospy.is_shutdown(): + return + + # Back to home + (result, state) = self.get_home_plan(state, False) + if rospy.is_shutdown(): + return + + return state + + +if __name__ == '__main__': + rospy.init_node("handring_parallel_planner") + handring_planner = HandringPlanner() + rospy.spin() diff --git a/motoman_demo/scripts/handring_parallel_planner_attached_object.py b/motoman_demo/scripts/handring_parallel_planner_attached_object.py new file mode 100755 index 0000000..2d18a24 --- /dev/null +++ b/motoman_demo/scripts/handring_parallel_planner_attached_object.py @@ -0,0 +1,445 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from time import sleep +# Basic +import sys +import copy +from math import * +import numpy as np +# ROS +import rospy +import rosparam +# Moveit +import moveit_commander +# TF +import tf2_ros +import tf +# == Messages == +# for Cartesian path +from geometry_msgs.msg import Pose +# for Pepper message +from std_msgs.msg import String +# for Start state +from moveit_msgs.msg import RobotState +from moveit_msgs.msg import RobotTrajectory +# for Planned path +from motoman_demo_msgs.msg import HandringPlan +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +# for object bounding box +from motoman_viz_msgs.msg import BoundingBoxArray +from motoman_viz_msgs.msg import BoundingBox +# == Service == +# for cleaning the Octomap +from std_srvs.srv import Empty + +class HandringPlanner(object): + + def __init__(self): + # ========= Subscriber ======== # + # self.speech_sub_topic = rospy.get_param('~speech') + self.speech_sub = rospy.Subscriber('/speech', String, self.speechCallback) + + # ========== Moveit init ========== # + # moveit_commander init + self.robot = moveit_commander.RobotCommander() + self.arm = moveit_commander.MoveGroupCommander("arm") + self.target_pose = Pose() + # Set the planning time + self.arm.set_planner_id('RRTConnectkConfigDefault') + self.planning_limitation_time = 5.0 + self.arm.set_planning_time(self.planning_limitation_time) + + # ========== TF ======== # + # TF Listner # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listner = tf2_ros.TransformListener(self.tf_buffer) + + # ========== Handring Plan publisher ======== # + self.hp_pub = rospy.Publisher('~handring_plan', HandringPlan, queue_size=6) + + # ========= Box Poses ======== # + self.box_pose = [{}, {}] + # Box 0 pose + self.box_pose[0]["joint_s"] = 0.33417996764183044 + self.box_pose[0]["joint_l"] = 0.6119770288467407 + self.box_pose[0]["joint_e"] = 0.0 + self.box_pose[0]["joint_u"] = 0.32670751214027405 + self.box_pose[0]["joint_r"] = 0.0 + self.box_pose[0]["joint_b"] = -0.6 + self.box_pose[0]["joint_t"] = 0.0 + # Box 1 pose + self.box_pose[1]["joint_s"] = 0.6041074967384338 + self.box_pose[1]["joint_l"] = 0.5769590139389038 + self.box_pose[1]["joint_e"] = -1.396013617515564 + self.box_pose[1]["joint_u"] = -0.6198193430900574 + self.box_pose[1]["joint_r"] = 0.0 + self.box_pose[1]["joint_b"] = -0.4 + self.box_pose[1]["joint_t"] = 0.0 + + # ======== Object Info ======== # + self.diff = rospy.get_param('~diff_from_object', 0.03) # diff from offset to grasp the object + self.offset = rospy.get_param('~offset', 0.45) # offset from top of the object + self.box_sub = rospy.Subscriber('/clustering_result', BoundingBoxArray, self.bbArrayCallback) + self.initial_box_num = 0 + + rospy.loginfo("HPP Initialized") + + # -------- Bounding Box Array call back -------- # + def bbArrayCallback(self, message): + self.initial_box_num = len(message.boxes) + + # -------- Get TF -------- # + def get_object_tf_data(self, num): + tf_time = rospy.Time(0) + target = "object_" + str(num) + get_tf_flg = False + while not get_tf_flg : + try : + trans = self.tf_buffer.lookup_transform('world', target, tf_time, rospy.Duration(10)) + get_tf_flg = True + + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) : + continue + return trans + + def get_goal_tf_data(self, object_trans, offset_x, offset_y): + goal_trans = copy.deepcopy(object_trans) + goal_trans.transform.translation.x = object_trans.transform.translation.x + offset_x + goal_trans.transform.translation.y = object_trans.transform.translation.y + offset_y + return goal_trans + + # -------- Clear Octomap -------- # + def clear_octomap(self): + rospy.wait_for_service('clear_octomap') + try: + result = rospy.ServiceProxy('clear_octomap', Empty) + result() + except rospy.ServiceException, e: + rospy.logwarn("Couldn't Clear Octomap") + + # -------- Plannning & Execution -------- # + def get_plan(self, trans, z_offset, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + z_offset + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + self.arm.set_pose_target(self.target_pose) + # plan + # for i in range(5): + # print "plan: " +str(i) + # plan = RobotTrajectory() + # counter = 0 + # while len(plan.joint_trajectory.points) == 0 : + # plan = self.arm.plan() + # counter+=1 + # self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + # if counter > 1 : + # return (False, start_state) + # self.arm.set_planning_time(self.planning_limitation_time) + # # sleep(1) + # rospy.sleep(0.5) + + #plan + # threshold = np.sqrt((moving_x*100)^2 + (moving_y*100)^2) + threshold = 20 + while(1): + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + if grasp == 0: + break + if len(plan.joint_trajectory.points) < threshold: + # print("---------debug0--------{}".format(len(plan.joint_trajectory.points))) + break + # print "input key A to continue." + # key = raw_input('>>> ') + # if key == "a": + # break + + # plan = RobotTrajectory() + # counter = 0 + # print("---------debug1--------{}".format(len(plan.joint_trajectory.points))) + # while len(plan.joint_trajectory.points) == 0 : + # plan = self.arm.plan() + # counter+=1 + # self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + # if counter > 1 : + # return (False, start_state) + # self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + # print("---------debug2--------{}".format(len(plan.joint_trajectory.points))) + pub_msg.trajectory = plan + + # print "input key A to continue." + # while(1): + # key = raw_input('>>> ') + # if key == "a": + # break + + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + def get_cartesian_plan(self, trans, z_offset, start_state, grasp): + # set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # set waypoints + waypoints = [] + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + wpose = Pose() + wpose.position = copy.deepcopy(self.target_pose.position) + wpose.orientation = copy.deepcopy(self.target_pose.orientation) + wpose.position.z = copy.deepcopy(self.target_pose.position.z + z_offset) + waypoints.append(wpose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + (plan, fraction) = self.arm.compute_cartesian_path( + waypoints, # waypoints to follow + 0.01, # eef_step + 0.0) # jump_threshold + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a cartesian plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + + # -------- Go to Home Position -------- # + def get_home_plan(self, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + init_pose = self.arm.get_current_joint_values() + init_pose[0] = 0.0 + init_pose[1] = 0.0 + init_pose[2] = 0.0 + init_pose[3] = 0.0 + init_pose[4] = 0.0 + init_pose[5] = 0.0 + init_pose[6] = 0.0 + self.arm.set_joint_value_target(init_pose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a home plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + # -------- Go to Box Position -------- # + def get_box_plan(self, num, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.arm.set_joint_value_target(self.box_pose[num]) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + rospy.loginfo("!! Got a box plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state, plan) + + # def set_plan(self, plan, grasp): + + # -------- Get message from pepper -------- # + def speechCallback(self, message): + rospy.loginfo("(-O-) Task start (-O-)") + # initialize + start_state = JointState() + start_state.header = Header() + start_state.header.stamp = rospy.Time.now() + start_state.name = rosparam.get_param("/controller_joint_names") + # start_state.name = rosparam.get_param("/sia5_controller/joints") + for i in range(len(start_state.name)): + start_state.position.append(0.) + get_num_from_pepper = int(message.data) + + # do the planning depending on the order number + if get_num_from_pepper == 99 : + rospy.loginfo("Called order 99") + rospy.loginfo("Currently this order is not working..") + object_trans = [] + goal_trans = [] + box_num = self.initial_box_num + rospy.loginfo("%d objects detected...", box_num) + for x in xrange(1, box_num+1): + trans = self.get_object_tf_data(x) + object_trans.append(trans) + goal_trans.append(self.get_goal_tf_data(trans, 0, 0.2)) #offset x, y + for x in xrange(0, box_num): + state = self.run(1, (x+1)%2, start_state, object_trans[x], goal_trans[x]) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + break + rospy.loginfo("No.%i task finished.", x+1) + start_state = state + rospy.loginfo("(^O^) All task finished (^O^)") + + else : + object_num = get_num_from_pepper / 10 + box_num = get_num_from_pepper % 10 - 1 + object_trans = self.get_object_tf_data(object_num) + moving_x = float(raw_input('axis:↓ offset_x[m] >>> ')) + moving_y = float(raw_input('axis:→ offset_y[m] >>> ')) + goal_trans = self.get_goal_tf_data(object_trans, moving_x, moving_y) #offset x, y + self.run(object_num, box_num, start_state, object_trans, goal_trans) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + rospy.loginfo("(^O^) All task finished (^O^)") + + # -------- Shutdown -------- # + def shutdown(self): + rospy.logwarn("(xOx) Aborted (xOx)") + + # -------- Run the Program -------- # + def run(self, obj_num, box_num, start_state, object_trans, goal_trans): + + # Go to Grasp + (result, state) = self.get_plan(object_trans, self.offset, start_state, False) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(object_trans, 0.3 + self.diff, state, True) + if rospy.is_shutdown(): + return + # Back to upper side + (result, state) = self.get_cartesian_plan(object_trans, self.offset + 0.05, state, True) + if rospy.is_shutdown(): + return + + # # Back to home + # (result, state) = self.get_home_plan(state, False) + # if rospy.is_shutdown(): + # return + + # Go to Release + (result, state) = self.get_plan(goal_trans, self.offset, state, True) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(goal_trans, 0.3 + self.diff, state, False) + if rospy.is_shutdown(): + return + + + # Back to upper side + (result, state) = self.get_cartesian_plan(goal_trans, self.offset + 0.05, state, False) + if rospy.is_shutdown(): + return + + # Back to home + (result, state) = self.get_home_plan(state, False) + if rospy.is_shutdown(): + return + + return state + + +if __name__ == '__main__': + rospy.init_node("handring_parallel_planner") + handring_planner = HandringPlanner() + rospy.spin() diff --git a/motoman_demo/scripts/handring_parallel_planner_for_pick_and_place.py b/motoman_demo/scripts/handring_parallel_planner_for_pick_and_place.py new file mode 100755 index 0000000..36e5944 --- /dev/null +++ b/motoman_demo/scripts/handring_parallel_planner_for_pick_and_place.py @@ -0,0 +1,440 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from time import sleep +# Basic +import sys +import copy +from math import * +import numpy as np +# ROS +import rospy +import rosparam +# Moveit +import moveit_commander +# TF +import tf2_ros +import tf +# == Messages == +# for Cartesian path +from geometry_msgs.msg import Pose +# for Pepper message +from std_msgs.msg import String +# for Start state +from moveit_msgs.msg import RobotState +from moveit_msgs.msg import RobotTrajectory +# for Planned path +from motoman_demo_msgs.msg import HandringPlan +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +# for object bounding box +from motoman_viz_msgs.msg import BoundingBoxArray +from motoman_viz_msgs.msg import BoundingBox +# for pick and place command +from motoman_interaction_msgs.msg import PickingInteraction +# == Service == +# for cleaning the Octomap +from std_srvs.srv import Empty + + +moving_x = 0 +moving_y = 0 + +class HandringPlanner(object): + + def __init__(self): + # ========= Subscriber ======== # + # self.speech_sub_topic = rospy.get_param('~speech') + self.speech_sub = rospy.Subscriber('/speech2', PickingInteraction, self.speechCallback) + + # ========== Moveit init ========== # + # moveit_commander init + self.robot = moveit_commander.RobotCommander() + self.arm = moveit_commander.MoveGroupCommander("arm") + self.target_pose = Pose() + # Set the planning time + self.arm.set_planner_id('RRTConnectkConfigDefault') + self.planning_limitation_time = 5.0 + self.arm.set_planning_time(self.planning_limitation_time) + + # ========== TF ======== # + # TF Listner # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listner = tf2_ros.TransformListener(self.tf_buffer) + + # ========== Handring Plan publisher ======== # + self.hp_pub = rospy.Publisher('~handring_plan', HandringPlan, queue_size=6) + + # ========= Box Poses ======== # + self.box_pose = [{}, {}] + # Box 0 pose + self.box_pose[0]["joint_s"] = 0.33417996764183044 + self.box_pose[0]["joint_l"] = 0.6119770288467407 + self.box_pose[0]["joint_e"] = 0.0 + self.box_pose[0]["joint_u"] = 0.32670751214027405 + self.box_pose[0]["joint_r"] = 0.0 + self.box_pose[0]["joint_b"] = -0.6 + self.box_pose[0]["joint_t"] = 0.0 + # Box 1 pose + self.box_pose[1]["joint_s"] = 0.6041074967384338 + self.box_pose[1]["joint_l"] = 0.5769590139389038 + self.box_pose[1]["joint_e"] = -1.396013617515564 + self.box_pose[1]["joint_u"] = -0.6198193430900574 + self.box_pose[1]["joint_r"] = 0.0 + self.box_pose[1]["joint_b"] = -0.4 + self.box_pose[1]["joint_t"] = 0.0 + + # ======== Object Info ======== # + self.diff = rospy.get_param('~diff_from_object', 0.03) # diff from offset to grasp the object + self.offset = rospy.get_param('~offset', 0.45) # offset from top of the object + self.box_sub = rospy.Subscriber('/clustering_result', BoundingBoxArray, self.bbArrayCallback) + self.initial_box_num = 0 + + rospy.loginfo("HPP Initialized") + + # -------- Bounding Box Array call back -------- # + def bbArrayCallback(self, message): + self.initial_box_num = len(message.boxes) + + # -------- Get TF -------- # + def get_object_tf_data(self, num): + tf_time = rospy.Time(0) + target = "object_" + str(num) + get_tf_flg = False + while not get_tf_flg : + try : + trans = self.tf_buffer.lookup_transform('world', target, tf_time, rospy.Duration(10)) + get_tf_flg = True + + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) : + continue + return trans + + def get_goal_tf_data(self, object_trans, offset_x, offset_y): + margin = 0.02 + dx = 0.08 + workspace_width = 0.60 + workspace_depth = 0.47 + dx + + goal_trans = copy.deepcopy(object_trans) + # target_x = object_trans.transform.translation.x + offset_x + dx + # target_y = object_trans.transform.translation.y + offset_y + target_x = offset_x + dx + target_y = offset_y + + print "target position (x, y): ("+str(round(target_x,3))+" ,"+str(round(target_y,3))+")" + if target_x < margin+dx or workspace_depth-margin < target_x or \ + target_y < -(workspace_width/2.0)+margin or (workspace_width/2.0)-margin < target_y: + rospy.logwarn("the target position is out of workspace!") + return False + else: + goal_trans.transform.translation.x = target_x + goal_trans.transform.translation.y = target_y + return goal_trans + + # -------- Clear Octomap -------- # + def clear_octomap(self): + rospy.wait_for_service('clear_octomap') + try: + result = rospy.ServiceProxy('clear_octomap', Empty) + result() + except rospy.ServiceException, e: + rospy.logwarn("Couldn't Clear Octomap") + + # -------- Plannning & Execution -------- # + def get_plan(self, trans, z_offset, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + z_offset + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + self.arm.set_pose_target(self.target_pose) + # plan + # for i in range(5): + # print "plan: " +str(i) + # plan = RobotTrajectory() + # counter = 0 + # while len(plan.joint_trajectory.points) == 0 : + # plan = self.arm.plan() + # counter+=1 + # self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + # if counter > 1 : + # return (False, start_state) + # self.arm.set_planning_time(self.planning_limitation_time) + # # sleep(1) + # rospy.sleep(0.5) + + #plan + # threshold = np.sqrt((moving_x*100)^2 + (moving_y*100)^2) + threshold = 20 + + while(1): + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + if grasp == 0: + break + if len(plan.joint_trajectory.points) < threshold: + # print("---------debug0--------{}".format(len(plan.joint_trajectory.points))) + break + + rospy.loginfo("!! Got a plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + # print("---------debug2--------{}".format(len(plan.joint_trajectory.points))) + pub_msg.trajectory = plan + + + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + def get_cartesian_plan(self, trans, z_offset, start_state, grasp): + # set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # set waypoints + waypoints = [] + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + wpose = Pose() + wpose.position = copy.deepcopy(self.target_pose.position) + wpose.orientation = copy.deepcopy(self.target_pose.orientation) + wpose.position.z = copy.deepcopy(self.target_pose.position.z + z_offset) + waypoints.append(wpose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + (plan, fraction) = self.arm.compute_cartesian_path( + waypoints, # waypoints to follow + 0.01, # eef_step + 0.0) # jump_threshold + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a cartesian plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + + # -------- Go to Home Position -------- # + def get_home_plan(self, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + init_pose = self.arm.get_current_joint_values() + init_pose[0] = 0.0 + init_pose[1] = 0.0 + init_pose[2] = 0.0 + init_pose[3] = 0.0 + init_pose[4] = 0.0 + init_pose[5] = 0.0 + init_pose[6] = 0.0 + self.arm.set_joint_value_target(init_pose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a home plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + # -------- Go to Box Position -------- # + def get_box_plan(self, num, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.arm.set_joint_value_target(self.box_pose[num]) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + rospy.loginfo("!! Got a box plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state, plan) + + # def set_plan(self, plan, grasp): + + # -------- Get message from pepper -------- # + def speechCallback(self, message): + rospy.loginfo("(-O-) Task start (-O-)") + # initialize + start_state = JointState() + start_state.header = Header() + start_state.header.stamp = rospy.Time.now() + start_state.name = rosparam.get_param("/controller_joint_names") + # start_state.name = rosparam.get_param("/sia5_controller/joints") + for i in range(len(start_state.name)): + start_state.position.append(0.) + + # do the planning depending on the order number + object_num = message.num + box_num = 1 + object_trans = self.get_object_tf_data(object_num) + moving_x = message.xm + moving_y = message.ym + goal_trans = self.get_goal_tf_data(object_trans, moving_x, moving_y) #offset x, y + + if goal_trans is False: + return + print + print "target: " + message.tag + print "↓ offset_x[m] : " +str(moving_x) + print "→ offse_ty[m] : " + str(moving_y) + + # print "input key A to continue." + # while(1): + # key = raw_input('>>> ') + # if key == "a": + # break + + self.run(object_num, box_num, start_state, object_trans, goal_trans) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + rospy.loginfo("(^O^) All task finished (^O^)") + + # -------- Shutdown -------- # + def shutdown(self): + rospy.logwarn("(xOx) Aborted (xOx)") + + # -------- Run the Program -------- # + def run(self, obj_num, box_num, start_state, object_trans, goal_trans): + + # Go to Grasp + (result, state) = self.get_plan(object_trans, self.offset, start_state, False) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(object_trans, 0.3 + self.diff, state, True) + if rospy.is_shutdown(): + return + # Back to upper side + (result, state) = self.get_cartesian_plan(object_trans, self.offset + 0.05, state, True) + if rospy.is_shutdown(): + return + + # # Back to home + # (result, state) = self.get_home_plan(state, False) + # if rospy.is_shutdown(): + # return + + # Go to Release + (result, state) = self.get_plan(goal_trans, self.offset, state, True) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(goal_trans, 0.3 + self.diff, state, False) + if rospy.is_shutdown(): + return + + + # Back to upper side + (result, state) = self.get_cartesian_plan(goal_trans, self.offset + 0.05, state, False) + if rospy.is_shutdown(): + return + + # Back to home + (result, state) = self.get_home_plan(state, False) + if rospy.is_shutdown(): + return + + return state + + +if __name__ == '__main__': + rospy.init_node("handring_parallel_planner") + handring_planner = HandringPlanner() + rospy.spin() diff --git a/motoman_demo/scripts/handring_parallel_planner_for_pick_and_place2.py b/motoman_demo/scripts/handring_parallel_planner_for_pick_and_place2.py new file mode 100755 index 0000000..3ad83df --- /dev/null +++ b/motoman_demo/scripts/handring_parallel_planner_for_pick_and_place2.py @@ -0,0 +1,439 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +from time import sleep +# Basic +import sys +import copy +from math import * +import numpy as np +# ROS +import rospy +import rosparam +# Moveit +import moveit_commander +# TF +import tf2_ros +import tf +# == Messages == +# for Cartesian path +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Transform +# for Pepper message +from std_msgs.msg import String +# for Start state +from moveit_msgs.msg import RobotState +from moveit_msgs.msg import RobotTrajectory +# for Planned path +from motoman_demo_msgs.msg import HandringPlan +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +# for object bounding box +from motoman_viz_msgs.msg import BoundingBoxArray +from motoman_viz_msgs.msg import BoundingBox +# for pick and place command +from motoman_interaction_msgs.msg import PickingInteraction +from motoman_interaction_msgs.msg import PoseArray + +# == Service == +# for cleaning the Octomap +from std_srvs.srv import Empty + + +moving_x = 0 +moving_y = 0 + +class HandringPlanner(object): + + def __init__(self): + # ========= Subscriber ======== # + # self.speech_sub_topic = rospy.get_param('~speech') + # self.speech_sub = rospy.Subscriber('/speech2', PickingInteraction, self.speechCallback) + self.speech_sub = rospy.Subscriber('/speech3', PoseArray, self.speechCallback) + + + # ========== Moveit init ========== # + # moveit_commander init + self.robot = moveit_commander.RobotCommander() + self.arm = moveit_commander.MoveGroupCommander("arm") + self.target_pose = Pose() + # Set the planning time + self.arm.set_planner_id('RRTConnectkConfigDefault') + self.planning_limitation_time = 5.0 + self.arm.set_planning_time(self.planning_limitation_time) + + # ========== TF ======== # + # TF Listner # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listner = tf2_ros.TransformListener(self.tf_buffer) + + # ========== Handring Plan publisher ======== # + self.hp_pub = rospy.Publisher('~handring_plan', HandringPlan, queue_size=6) + + # ========= Box Poses ======== # + self.box_pose = [{}, {}] + # Box 0 pose + self.box_pose[0]["joint_s"] = 0.33417996764183044 + self.box_pose[0]["joint_l"] = 0.6119770288467407 + self.box_pose[0]["joint_e"] = 0.0 + self.box_pose[0]["joint_u"] = 0.32670751214027405 + self.box_pose[0]["joint_r"] = 0.0 + self.box_pose[0]["joint_b"] = -0.6 + self.box_pose[0]["joint_t"] = 0.0 + # Box 1 pose + self.box_pose[1]["joint_s"] = 0.6041074967384338 + self.box_pose[1]["joint_l"] = 0.5769590139389038 + self.box_pose[1]["joint_e"] = -1.396013617515564 + self.box_pose[1]["joint_u"] = -0.6198193430900574 + self.box_pose[1]["joint_r"] = 0.0 + self.box_pose[1]["joint_b"] = -0.4 + self.box_pose[1]["joint_t"] = 0.0 + + # ======== Object Info ======== # + self.diff = rospy.get_param('~diff_from_object', 0.03) # diff from offset to grasp the object + self.offset = rospy.get_param('~offset', 0.45) # offset from top of the object + self.box_sub = rospy.Subscriber('/clustering_result', BoundingBoxArray, self.bbArrayCallback) + self.initial_box_num = 0 + + rospy.loginfo("HPP Initialized") + + # -------- Bounding Box Array call back -------- # + def bbArrayCallback(self, message): + self.initial_box_num = len(message.boxes) + + # -------- Get TF -------- # + def get_object_tf_data(self, initial_pose): + # tf_time = rospy.Time(0) + trans = Transform() + trans.translation = copy.deepcopy(initial_pose.position) + trans.translation.z = 0.105 + trans.rotation = copy.deepcopy(initial_pose.orientation) + return trans + + def get_goal_tf_data(self, object_trans, goal_pose): + margin = 0.02 + dx = 0.08 + workspace_width = 0.60 + workspace_depth = 0.47 + dx + + goal_trans = copy.deepcopy(object_trans) + # target_x = object_trans.translation.x + offset_x + dx + # target_y = object_trans.translation.y + offset_y + target_x = goal_pose.position.x + dx + target_y = goal_pose.position.y + + print "target position (x, y): ("+str(round(target_x,3))+" ,"+str(round(target_y,3))+")" + if target_x < margin+dx or workspace_depth-margin < target_x or \ + target_y < -(workspace_width/2.0)+margin or (workspace_width/2.0)-margin < target_y: + rospy.logwarn("the target position is out of workspace!") + return False + else: + goal_trans.translation.x = target_x + goal_trans.translation.y = target_y + return goal_trans + + # -------- Clear Octomap -------- # + def clear_octomap(self): + rospy.wait_for_service('clear_octomap') + try: + result = rospy.ServiceProxy('clear_octomap', Empty) + result() + except rospy.ServiceException, e: + rospy.logwarn("Couldn't Clear Octomap") + + # -------- Plannning & Execution -------- # + def get_plan(self, trans, z_offset, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.target_pose.position.x = trans.translation.x + self.target_pose.position.y = trans.translation.y + self.target_pose.position.z = trans.translation.z + z_offset + # self.target_pose.position.z = 0.15 + z_offset + + q = (trans.rotation.x, + trans.rotation.y, + trans.rotation.z, + trans.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + self.arm.set_pose_target(self.target_pose) + # plan + # for i in range(5): + # print "plan: " +str(i) + # plan = RobotTrajectory() + # counter = 0 + # while len(plan.joint_trajectory.points) == 0 : + # plan = self.arm.plan() + # counter+=1 + # self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + # if counter > 1 : + # return (False, start_state) + # self.arm.set_planning_time(self.planning_limitation_time) + # # sleep(1) + # rospy.sleep(0.5) + + #plan + # threshold = np.sqrt((moving_x*100)^2 + (moving_y*100)^2) + threshold = 20 + + while(1): + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + if grasp == 0: + break + if len(plan.joint_trajectory.points) < threshold: + # print("---------debug0--------{}".format(len(plan.joint_trajectory.points))) + break + + rospy.loginfo("!! Got a plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + # print("---------debug2--------{}".format(len(plan.joint_trajectory.points))) + pub_msg.trajectory = plan + + + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + def get_cartesian_plan(self, trans, z_offset, start_state, grasp): + # set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # set waypoints + waypoints = [] + self.target_pose.position.x = trans.translation.x + self.target_pose.position.y = trans.translation.y + self.target_pose.position.z = trans.translation.z + q = (trans.rotation.x, + trans.rotation.y, + trans.rotation.z, + trans.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + wpose = Pose() + wpose.position = copy.deepcopy(self.target_pose.position) + wpose.orientation = copy.deepcopy(self.target_pose.orientation) + wpose.position.z = copy.deepcopy(self.target_pose.position.z + z_offset) + waypoints.append(wpose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + (plan, fraction) = self.arm.compute_cartesian_path( + waypoints, # waypoints to follow + 0.01, # eef_step + 0.0) # jump_threshold + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a cartesian plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + + # -------- Go to Home Position -------- # + def get_home_plan(self, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + init_pose = self.arm.get_current_joint_values() + init_pose[0] = 0.0 + init_pose[1] = 0.0 + init_pose[2] = 0.0 + init_pose[3] = 0.0 + init_pose[4] = 0.0 + init_pose[5] = 0.0 + init_pose[6] = 0.0 + self.arm.set_joint_value_target(init_pose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a home plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + # -------- Go to Box Position -------- # + def get_box_plan(self, num, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.arm.set_joint_value_target(self.box_pose[num]) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + rospy.loginfo("!! Got a box plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state, plan) + + # def set_plan(self, plan, grasp): + + # -------- Get message from pepper -------- # + def speechCallback(self, message): + rospy.loginfo("(-O-) Task start (-O-)") + # # initialize + start_state = JointState() + start_state.header = Header() + start_state.header.stamp = rospy.Time.now() + start_state.name = rosparam.get_param("/controller_joint_names") + # start_state.name = rosparam.get_param("/sia5_controller/joints") + for i in range(len(start_state.name)): + start_state.position.append(0.) + + # # do the planning + + task_N = len(message.goal_pose) + for i in range(task_N): + object_trans = self.get_object_tf_data(message.initial_pose[i]) + goal_trans = self.get_goal_tf_data(object_trans, message.goal_pose[i]) + + if i == 0: + state = self.run(start_state, object_trans, goal_trans) + else: + state = self.run(state, object_trans, goal_trans) + + # # print "input key A to continue." + # # while(1): + # # key = raw_input('>>> ') + # # if key == "a": + # # break + + + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + rospy.loginfo("(^O^) All task finished (^O^)") + + # -------- Shutdown -------- # + def shutdown(self): + rospy.logwarn("(xOx) Aborted (xOx)") + + # -------- Run the Program -------- # + def run(self, start_state, object_trans, goal_trans): + + # Go to Grasp + (result, state) = self.get_plan(object_trans, self.offset, start_state, False) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(object_trans, 0.3 + self.diff, state, True) + if rospy.is_shutdown(): + return + # Back to upper side + (result, state) = self.get_cartesian_plan(object_trans, self.offset + 0.05, state, True) + if rospy.is_shutdown(): + return + + # # Back to home + # (result, state) = self.get_home_plan(state, False) + # if rospy.is_shutdown(): + # return + + # Go to Release + (result, state) = self.get_plan(goal_trans, self.offset, state, True) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(goal_trans, 0.3 + self.diff, state, False) + if rospy.is_shutdown(): + return + + + # Back to upper side + (result, state) = self.get_cartesian_plan(goal_trans, self.offset + 0.05, state, False) + if rospy.is_shutdown(): + return + + # Back to home + (result, state) = self.get_home_plan(state, False) + if rospy.is_shutdown(): + return + + return state + + +if __name__ == '__main__': + rospy.init_node("handring_parallel_planner") + handring_planner = HandringPlanner() + rospy.spin() diff --git a/motoman_demo/scripts/handring_parallel_planner_sidebox.py b/motoman_demo/scripts/handring_parallel_planner_sidebox.py new file mode 100755 index 0000000..b93db14 --- /dev/null +++ b/motoman_demo/scripts/handring_parallel_planner_sidebox.py @@ -0,0 +1,372 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# Basic +import sys +import copy +from math import * +# ROS +import rospy +import rosparam +# Moveit +import moveit_commander +# TF +import tf2_ros +import tf +# == Messages == +# for Cartesian path +from geometry_msgs.msg import Pose +# for Pepper message +from std_msgs.msg import String +# for Start state +from moveit_msgs.msg import RobotState +from moveit_msgs.msg import RobotTrajectory +# for Planned path +from motoman_demo_msgs.msg import HandringPlan +from sensor_msgs.msg import JointState +from std_msgs.msg import Header +# for object bounding box +from motoman_viz_msgs.msg import BoundingBoxArray +from motoman_viz_msgs.msg import BoundingBox +# == Service == +# for cleaning the Octomap +from std_srvs.srv import Empty + +class HandringPlanner(object): + + def __init__(self): + # ========= Subscriber ======== # + # self.speech_sub_topic = rospy.get_param('~speech') + self.speech_sub = rospy.Subscriber('/speech', String, self.speechCallback) + + # ========== Moveit init ========== # + # moveit_commander init + self.robot = moveit_commander.RobotCommander() + self.arm = moveit_commander.MoveGroupCommander("arm") + self.target_pose = Pose() + # Set the planning time + self.arm.set_planner_id('RRTConnectkConfigDefault') + self.planning_limitation_time = 5.0 + self.arm.set_planning_time(self.planning_limitation_time) + + # ========== TF ======== # + # TF Listner # + self.tf_buffer = tf2_ros.Buffer() + self.tf_listner = tf2_ros.TransformListener(self.tf_buffer) + + # ========== Handring Plan publisher ======== # + self.hp_pub = rospy.Publisher('~handring_plan', HandringPlan, queue_size=6) + + # ========= Box Poses ======== # + self.box_pose = [{}, {}] + # Box 0 pose + self.box_pose[0]["joint_s"] = -0.6041074967384338 + self.box_pose[0]["joint_l"] = 0.576959013938903 + self.box_pose[0]["joint_e"] = 1.396013617515564 + self.box_pose[0]["joint_u"] = -0.6198193430900574 + self.box_pose[0]["joint_r"] = 0.0 + self.box_pose[0]["joint_b"] = -0.4 + self.box_pose[0]["joint_t"] = 0.0 + + # Box 1 pose + self.box_pose[1]["joint_s"] = 0.6041074967384338 + self.box_pose[1]["joint_l"] = 0.5769590139389038 + self.box_pose[1]["joint_e"] = -1.396013617515564 + self.box_pose[1]["joint_u"] = -0.6198193430900574 + self.box_pose[1]["joint_r"] = 0.0 + self.box_pose[1]["joint_b"] = -0.4 + self.box_pose[1]["joint_t"] = 0.0 + + # ======== Object Info ======== # + self.diff = rospy.get_param('~diff_from_object', 0.03) # diff from offset to grasp the object + self.offset = rospy.get_param('~offset', 0.45) # offset from top of the object + self.box_sub = rospy.Subscriber('/clustering_result', BoundingBoxArray, self.bbArrayCallback) + self.initial_box_num = 0 + + rospy.loginfo("HPP Initialized") + + # -------- Bounding Box Array call back -------- # + def bbArrayCallback(self, message): + self.initial_box_num = len(message.boxes) + + # -------- Get TF -------- # + def get_tf_data(self, num): + tf_time = rospy.Time(0) + target = "object_" + str(num) + get_tf_flg = False + while not get_tf_flg : + try : + trans = self.tf_buffer.lookup_transform('world', target, tf_time, rospy.Duration(10)) + get_tf_flg = True + + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) : + continue + return trans + + # -------- Clear Octomap -------- # + def clear_octomap(self): + rospy.wait_for_service('clear_octomap') + try: + result = rospy.ServiceProxy('clear_octomap', Empty) + result() + except rospy.ServiceException, e: + rospy.logwarn("Couldn't Clear Octomap") + + # -------- Plannning & Execution -------- # + def get_plan(self, trans, z_offset, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + z_offset + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + self.arm.set_pose_target(self.target_pose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + def get_cartesian_plan(self, trans, z_offset, start_state, grasp): + # set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # set waypoints + waypoints = [] + self.target_pose.position.x = trans.transform.translation.x + self.target_pose.position.y = trans.transform.translation.y + self.target_pose.position.z = trans.transform.translation.z + q = (trans.transform.rotation.x, + trans.transform.rotation.y, + trans.transform.rotation.z, + trans.transform.rotation.w) + (roll,pitch,yaw) = tf.transformations.euler_from_quaternion(q) + pitch += pi/2.0 + tar_q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) + self.target_pose.orientation.x = tar_q[0] + self.target_pose.orientation.y = tar_q[1] + self.target_pose.orientation.z = tar_q[2] + self.target_pose.orientation.w = tar_q[3] + wpose = Pose() + wpose.position = copy.deepcopy(self.target_pose.position) + wpose.orientation = copy.deepcopy(self.target_pose.orientation) + wpose.position.z = copy.deepcopy(self.target_pose.position.z + z_offset) + waypoints.append(wpose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + (plan, fraction) = self.arm.compute_cartesian_path( + waypoints, # waypoints to follow + 0.01, # eef_step + 0.0) # jump_threshold + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a cartesian plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + + # -------- Go to Home Position -------- # + def get_home_plan(self, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + init_pose = self.arm.get_current_joint_values() + init_pose[0] = 0.0 + init_pose[1] = 0.0 + init_pose[2] = 0.0 + init_pose[3] = 0.0 + init_pose[4] = 0.0 + init_pose[5] = 0.0 + init_pose[6] = 0.0 + self.arm.set_joint_value_target(init_pose) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + + rospy.loginfo("!! Got a home plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state) + + # -------- Go to Box Position -------- # + def get_box_plan(self, num, start_state, grasp): + # Set argument start state + moveit_start_state = RobotState() + moveit_start_state.joint_state = start_state + self.arm.set_start_state(moveit_start_state) + # Calculate goal pose + self.arm.set_joint_value_target(self.box_pose[num]) + # plan + plan = RobotTrajectory() + counter = 0 + while len(plan.joint_trajectory.points) == 0 : + plan = self.arm.plan() + counter+=1 + self.arm.set_planning_time(self.planning_limitation_time+counter*5.0) + if counter > 1 : + return (False, start_state) + self.arm.set_planning_time(self.planning_limitation_time) + rospy.loginfo("!! Got a box plan !!") + # publish the plan + pub_msg = HandringPlan() + pub_msg.grasp = grasp + pub_msg.trajectory = plan + self.hp_pub.publish(pub_msg) + self.arm.clear_pose_targets() + + # return goal state from generated trajectory + goal_state = JointState() + goal_state.header = Header() + goal_state.header.stamp = rospy.Time.now() + goal_state.name = plan.joint_trajectory.joint_names[:] + goal_state.position = plan.joint_trajectory.points[-1].positions[:] + return (True, goal_state, plan) + + # def set_plan(self, plan, grasp): + + # -------- Get message from pepper -------- # + def speechCallback(self, message): + rospy.loginfo("(-O-) Task start (-O-)") + # initialize + start_state = JointState() + start_state.header = Header() + start_state.header.stamp = rospy.Time.now() + start_state.name = rosparam.get_param("/controller_joint_names") + # start_state.name = rosparam.get_param("/sia5_controller/joints") + for i in range(len(start_state.name)): + start_state.position.append(0.) + get_num_from_pepper = int(message.data) + + # do the planning depending on the order number + if get_num_from_pepper == 99 : + rospy.loginfo("Called order 99") + trans = [] + box_num = self.initial_box_num + rospy.loginfo("%d objects detected...", box_num) + for x in xrange(1, box_num+1): + trans.append(self.get_tf_data(x)) + for x in xrange(0, box_num): + state = self.run(1, (x+1)%2, start_state, trans[x]) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + break + rospy.loginfo("No.%i task finished.", x+1) + start_state = state + rospy.loginfo("(^O^) All task finished (^O^)") + + else : + object_num = get_num_from_pepper / 10 + box_num = get_num_from_pepper % 10 - 1 + trans = self.get_tf_data(object_num) + self.run(object_num, box_num, start_state, trans) + if rospy.is_shutdown(): + rospy.on_shutdown(self.shutdown) + rospy.loginfo("(^O^) All task finished (^O^)") + + # -------- Shutdown -------- # + def shutdown(self): + rospy.logwarn("(xOx) Aborted (xOx)") + + # -------- Run the Program -------- # + def run(self, obj_num, box_num, start_state, trans): + # Go to Grasp + (result, state) = self.get_plan(trans, self.offset, start_state, False) + if rospy.is_shutdown(): + return + (result, state) = self.get_cartesian_plan(trans, 0.3 + self.diff, state, True) + if rospy.is_shutdown(): + return + # Back to upper side + (result, state) = self.get_cartesian_plan(trans, self.offset + 0.05, state, True) + if rospy.is_shutdown(): + return + # Back to home + (result, state) = self.get_home_plan(state, True) + if rospy.is_shutdown(): + return + # Go to Box + (result, state, plan) = self.get_box_plan(box_num, state, False) + if rospy.is_shutdown(): + return + # Go to home + (result, state) = self.get_home_plan(state, False) + if rospy.is_shutdown(): + return + + return state + + +if __name__ == '__main__': + rospy.init_node("handring_parallel_planner") + handring_planner = HandringPlanner() + rospy.spin() diff --git a/motoman_description/robots/sia5/sia5_with_dhand_and_3_kinects.urdf.xacro b/motoman_description/robots/sia5/sia5_with_dhand_and_3_kinects.urdf.xacro new file mode 100644 index 0000000..18a4ba2 --- /dev/null +++ b/motoman_description/robots/sia5/sia5_with_dhand_and_3_kinects.urdf.xacro @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motoman_description/robots/sia5/sia5_with_dhand_and_4_kinects.urdf.xacro b/motoman_description/robots/sia5/sia5_with_dhand_and_4_kinects.urdf.xacro index 1e01afa..4d85646 100644 --- a/motoman_description/robots/sia5/sia5_with_dhand_and_4_kinects.urdf.xacro +++ b/motoman_description/robots/sia5/sia5_with_dhand_and_4_kinects.urdf.xacro @@ -6,26 +6,27 @@ - + - - + + - - + + + - + - + - - + + - + diff --git a/motoman_description/robots/sia5/sia5_with_dhand_and_multi_kinect.urdf.xacro b/motoman_description/robots/sia5/sia5_with_dhand_and_multi_kinect.urdf.xacro index 69c0411..693e18a 100644 --- a/motoman_description/robots/sia5/sia5_with_dhand_and_multi_kinect.urdf.xacro +++ b/motoman_description/robots/sia5/sia5_with_dhand_and_multi_kinect.urdf.xacro @@ -6,26 +6,26 @@ - + - - + + - - + + - + - + - - + + - + @@ -48,7 +48,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - diff --git a/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/fake_controllers.yaml b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/fake_controllers.yaml new file mode 100644 index 0000000..0f390f8 --- /dev/null +++ b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/fake_controllers.yaml @@ -0,0 +1,20 @@ +controller_list: + - name: fake_arm_controller + joints: + - joint_s + - joint_l + - joint_e + - joint_u + - joint_r + - joint_b + - joint_t + - name: fake_gripper_controller + joints: + - dhand_finger_base_left_joint + - dhand_finger_middle_left_joint + - dhand_finger_top_left_joint + - dhand_finger_middle_middle_joint + - dhand_finger_top_middle_joint + - dhand_finger_base_right_joint + - dhand_finger_middle_right_joint + - dhand_finger_top_right_joint \ No newline at end of file diff --git a/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/joint_limits.yaml b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/joint_limits.yaml new file mode 100644 index 0000000..ee277fd --- /dev/null +++ b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/joint_limits.yaml @@ -0,0 +1,79 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + dhand_finger_base_left_joint: + has_velocity_limits: true + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 + dhand_finger_base_right_joint: + has_velocity_limits: true + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 + dhand_finger_middle_left_joint: + has_velocity_limits: true + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 + dhand_finger_middle_middle_joint: + has_velocity_limits: true + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 + dhand_finger_middle_right_joint: + has_velocity_limits: true + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 + dhand_finger_top_left_joint: + has_velocity_limits: true + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 + dhand_finger_top_middle_joint: + has_velocity_limits: true + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 + dhand_finger_top_right_joint: + has_velocity_limits: true + max_velocity: 10 + has_acceleration_limits: false + max_acceleration: 0 + joint_b: + has_velocity_limits: true + max_velocity: 3.8135445205 + has_acceleration_limits: false + max_acceleration: 0 + joint_e: + has_velocity_limits: true + max_velocity: 3.31612567 + has_acceleration_limits: false + max_acceleration: 0 + joint_l: + has_velocity_limits: true + max_velocity: 3.31612567 + has_acceleration_limits: false + max_acceleration: 0 + joint_r: + has_velocity_limits: true + max_velocity: 3.31612567 + has_acceleration_limits: false + max_acceleration: 0 + joint_s: + has_velocity_limits: true + max_velocity: 3.31612567 + has_acceleration_limits: false + max_acceleration: 0 + joint_t: + has_velocity_limits: true + max_velocity: 5.8032199225 + has_acceleration_limits: false + max_acceleration: 0 + joint_u: + has_velocity_limits: true + max_velocity: 3.31612567 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/kinect2.yaml b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/kinect2.yaml new file mode 100644 index 0000000..f61adfb --- /dev/null +++ b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/kinect2.yaml @@ -0,0 +1,8 @@ +sensors: + - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater + point_cloud_topic: /merged_cloud + max_range: 2.0 + point_subsample: 1 + padding_offset: 0.2 + padding_scale: 1.0 + filtered_cloud_topic: filtered_cloud diff --git a/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/kinematics.yaml b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/kinematics.yaml new file mode 100644 index 0000000..a1f17d0 --- /dev/null +++ b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/kinematics.yaml @@ -0,0 +1,5 @@ +arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 \ No newline at end of file diff --git a/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/ompl_planning.yaml b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/ompl_planning.yaml new file mode 100644 index 0000000..42216b0 --- /dev/null +++ b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/ompl_planning.yaml @@ -0,0 +1,84 @@ +planner_configs: + SBLkConfigDefault: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ESTkConfigDefault: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECEkConfigDefault: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECEkConfigDefault: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECEkConfigDefault: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRTkConfigDefault: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnectkConfigDefault: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstarkConfigDefault: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRTkConfigDefault: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRMkConfigDefault: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstarkConfigDefault: + type: geometric::PRMstar +arm: + default_planner_config: RRTConnectkConfigDefault + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault + projection_evaluator: joints(joint_s,joint_l) + longest_valid_segment_fraction: 0.05 +gripper: + default_planner_config: RRTConnectkConfigDefault + planner_configs: + - SBLkConfigDefault + - ESTkConfigDefault + - LBKPIECEkConfigDefault + - BKPIECEkConfigDefault + - KPIECEkConfigDefault + - RRTkConfigDefault + - RRTConnectkConfigDefault + - RRTstarkConfigDefault + - TRRTkConfigDefault + - PRMkConfigDefault + - PRMstarkConfigDefault \ No newline at end of file diff --git a/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/sia5.srdf b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/sia5.srdf new file mode 100644 index 0000000..bcd29e6 --- /dev/null +++ b/motoman_moveit/config/sia5_with_dhand_and_multi_kinect/sia5.srdf @@ -0,0 +1,226 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/motoman_moveit/launch/sda5/rviz/moveit_sda5.rviz b/motoman_moveit/launch/sda5/rviz/moveit_sda5.rviz index 4632564..e57d9c3 100644 --- a/motoman_moveit/launch/sda5/rviz/moveit_sda5.rviz +++ b/motoman_moveit/launch/sda5/rviz/moveit_sda5.rviz @@ -6,11 +6,15 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /TF1 - /TF1/Frames1 - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 + - /PointCloud21 + - /BoundingBoxArray1 + - /PointCloud22 Splitter Ratio: 0.5 - Tree Height: 283 + Tree Height: 270 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -29,7 +33,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -205,13 +209,17 @@ Visualization Manager: Value: false dhand_palm_link: Value: false - kinect_first_link: + kinect_center_link: Value: true - kinect_first_rgb_optical_frame: + kinect_center_rgb_optical_frame: Value: true - kinect_second_link: + kinect_left_link: Value: true - kinect_second_rgb_optical_frame: + kinect_left_rgb_optical_frame: + Value: true + kinect_right_link: + Value: true + kinect_right_rgb_optical_frame: Value: true link_b: Value: false @@ -229,22 +237,6 @@ Visualization Manager: Value: false object_1: Value: true - object_2: - Value: true - object_3: - Value: true - object_4: - Value: true - object_5: - Value: true - object_6: - Value: true - object_7: - Value: true - object_8: - Value: true - object_9: - Value: true tool0: Value: false world: @@ -257,11 +249,14 @@ Visualization Manager: Tree: world: base_link: - kinect_first_link: - kinect_first_rgb_optical_frame: + kinect_center_link: + kinect_center_rgb_optical_frame: + {} + kinect_left_link: + kinect_left_rgb_optical_frame: {} - kinect_second_link: - kinect_second_rgb_optical_frame: + kinect_right_link: + kinect_right_rgb_optical_frame: {} link_s: link_l: @@ -289,22 +284,6 @@ Visualization Manager: {} object_1: {} - object_2: - {} - object_3: - {} - object_4: - {} - object_5: - {} - object_6: - {} - object_7: - {} - object_8: - {} - object_9: - {} Update Interval: 0 Value: true - Class: moveit_rviz_plugin/MotionPlanning @@ -605,18 +584,6 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: jsk_rviz_plugin/BoundingBoxArray - Enabled: true - Name: BoundingBoxArray - Topic: /clustering_result - Unreliable: false - Value: true - alpha: 0.8 - color: 25; 255; 0 - coloring: Auto - line width: 0.005 - only edge: false - show coords: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -629,7 +596,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -642,11 +609,11 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /kinect_first/hd/points - Unreliable: false + Topic: /kinect_center/qhd/points + Unreliable: true Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Class: motoman_rviz_plugin/EuclideanLinkTrajectory Enabled: true Name: EuclideanLinkTrajectory1 @@ -717,6 +684,48 @@ Visualization Manager: color: 255; 0; 0 line_width: 0.01 link name: link_t + - Class: motoman_rviz_plugin/BoundingBoxArray + Enabled: true + Name: BoundingBoxArray + Topic: /clustering_result + Unreliable: false + Value: true + alpha: 0.8 + color: 25; 255; 0 + coloring: Auto + line width: 0.005 + only edge: false + show coords: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /kinect_center/qhd/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -741,32 +750,32 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.98459 + Distance: 2.16969 Enable Stereo Rendering: Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.140364 - Y: 0.239096 - Z: 0.392613 + X: 0.401247 + Y: 0.0423675 + Z: 0.34002 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.340399 + Pitch: 0.4354 Target Frame: world Value: Orbit (rviz) - Yaw: 5.89358 + Yaw: 5.90357 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1056 + Height: 1028 Hide Left Dock: false Hide Right Dock: true MotionPlanning: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002c100000396fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000028000001aa000000dd00fffffffa000000000100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000016a00fffffffb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000000ffffffff0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001d8000001e6000001cc00ffffff000000010000010f000006e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000006e0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004b90000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000002c10000037afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc000000280000019d000000dd00fffffffa000000000100000002fb000000100044006900730070006c0061007900730100000000ffffffff0000016a00fffffffb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000000ffffffff0000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001cb000001d7000001cc00ffffff000000010000010f000006e0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000006e0000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000074d0000003efc0100000002fb0000000800540069006d006501000000000000074d000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004860000037a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -775,6 +784,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1920 - X: 1910 - Y: -14 + Width: 1869 + X: 1941 + Y: 14 diff --git a/motoman_moveit/launch/sia5/rviz/moveit_sia5.rviz b/motoman_moveit/launch/sia5/rviz/moveit_sia5.rviz index 4632564..9afa147 100644 --- a/motoman_moveit/launch/sia5/rviz/moveit_sia5.rviz +++ b/motoman_moveit/launch/sia5/rviz/moveit_sia5.rviz @@ -9,6 +9,8 @@ Panels: - /TF1/Frames1 - /MotionPlanning1/Planning Request1 - /MotionPlanning1/Planned Path1 + - /PointCloud21 + - /BoundingBoxArray1 Splitter Ratio: 0.5 Tree Height: 283 - Class: rviz/Selection @@ -29,7 +31,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: PointCloud2 Visualization Manager: Class: "" Displays: @@ -205,13 +207,17 @@ Visualization Manager: Value: false dhand_palm_link: Value: false - kinect_first_link: + kinect_center_link: Value: true - kinect_first_rgb_optical_frame: + kinect_center_rgb_optical_frame: Value: true - kinect_second_link: + kinect_left_link: Value: true - kinect_second_rgb_optical_frame: + kinect_left_rgb_optical_frame: + Value: true + kinect_right_link: + Value: true + kinect_right_rgb_optical_frame: Value: true link_b: Value: false @@ -231,20 +237,6 @@ Visualization Manager: Value: true object_2: Value: true - object_3: - Value: true - object_4: - Value: true - object_5: - Value: true - object_6: - Value: true - object_7: - Value: true - object_8: - Value: true - object_9: - Value: true tool0: Value: false world: @@ -257,11 +249,14 @@ Visualization Manager: Tree: world: base_link: - kinect_first_link: - kinect_first_rgb_optical_frame: + kinect_center_link: + kinect_center_rgb_optical_frame: {} - kinect_second_link: - kinect_second_rgb_optical_frame: + kinect_left_link: + kinect_left_rgb_optical_frame: + {} + kinect_right_link: + kinect_right_rgb_optical_frame: {} link_s: link_l: @@ -291,20 +286,6 @@ Visualization Manager: {} object_2: {} - object_3: - {} - object_4: - {} - object_5: - {} - object_6: - {} - object_7: - {} - object_8: - {} - object_9: - {} Update Interval: 0 Value: true - Class: moveit_rviz_plugin/MotionPlanning @@ -605,18 +586,6 @@ Visualization Manager: {} Queue Size: 100 Value: true - - Class: jsk_rviz_plugin/BoundingBoxArray - Enabled: true - Name: BoundingBoxArray - Topic: /clustering_result - Unreliable: false - Value: true - alpha: 0.8 - color: 25; 255; 0 - coloring: Auto - line width: 0.005 - only edge: false - show coords: false - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -629,7 +598,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -642,11 +611,11 @@ Visualization Manager: Size (Pixels): 3 Size (m): 0.01 Style: Flat Squares - Topic: /kinect_first/hd/points + Topic: /kinect_center/qhd/points Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Class: motoman_rviz_plugin/EuclideanLinkTrajectory Enabled: true Name: EuclideanLinkTrajectory1 @@ -717,6 +686,18 @@ Visualization Manager: color: 255; 0; 0 line_width: 0.01 link name: link_t + - Class: motoman_rviz_plugin/BoundingBoxArray + Enabled: true + Name: BoundingBoxArray + Topic: /clustering_result + Unreliable: false + Value: true + alpha: 0.8 + color: 25; 255; 0 + coloring: Auto + line width: 0.005 + only edge: false + show coords: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -753,10 +734,10 @@ Visualization Manager: Z: 0.392613 Name: Current View Near Clip Distance: 0.01 - Pitch: 0.340399 + Pitch: 0.040399 Target Frame: world Value: Orbit (rviz) - Yaw: 5.89358 + Yaw: 0.0953822 Saved: ~ Window Geometry: Displays: @@ -776,5 +757,5 @@ Window Geometry: Views: collapsed: true Width: 1920 - X: 1910 - Y: -14 + X: 1920 + Y: 24 diff --git a/motoman_moveit/scene/exihibition_2016_sidebox.scene b/motoman_moveit/scene/exihibition_2016_sidebox.scene new file mode 100644 index 0000000..040e7bf --- /dev/null +++ b/motoman_moveit/scene/exihibition_2016_sidebox.scene @@ -0,0 +1,74 @@ +Nishida Lab motoman work +* Save Object +3 +box +0.2 0.16 0.125 +-0.23 0 0.065 +0 0 0 1 +1 0.5 0.5 0 +box +0.20 0.66 1.56 +-0.45 0 0.78 +0 0 0 1 +1 0.5 0.5 0 +box +1.1 0.66 0.16 +0 0 1.48 +0 0 0 1 +1 0.5 0.5 0 +* Wall +1 +box +1.74 0.1 2.13 +0.045 -0.8 0.555 +0 0 0 1 +1 0.5 0.5 0 +* Save Table +3 +box +1.1 0.24 0.05 +0 0.21 0.025 +0 0 0 1 +1 0.5 0.5 0 +box +1.1 0.24 0.05 +0 -0.21 0.025 +0 0 0 1 +1 0.5 0.5 0 +box +0.47 0.18 0.05 +0.315 0 0.025 +0 0 0 1 +1 0.5 0.5 0 +* Work table +1 +box +1.1 0.66 0.04 +0 0 -0.02 +0 0 0 1 +0.5 0.5 0.5 1 +* Pole +2 +box +0.04 0.04 1.6 +-0.552 0 0.8 +0 0 0 1 +0.5 0.5 0.5 1 +box +1.1 0.04 0.04 +0 0 1.58 +0 0 0 1 +0.5 0.5 0.5 1 +* Drop box +2 +box +0.47 0.37 0.64 +0.315 -0.515 -0.19 +0 0 0 1 +0.231 0.305 0.258 0.5 +box +0.47 0.37 0.64 +0.315 0.515 -0.19 +0 0 0 1 +0.231 0.305 0.258 0.5 +. diff --git a/motoman_moveit/scene/exihibition_2016_sidebox_and_wall.scene b/motoman_moveit/scene/exihibition_2016_sidebox_and_wall.scene new file mode 100644 index 0000000..aac9c9f --- /dev/null +++ b/motoman_moveit/scene/exihibition_2016_sidebox_and_wall.scene @@ -0,0 +1,74 @@ +Nishida Lab motoman work +* Save Object +3 +box +0.2 0.16 0.125 +-0.23 0 0.065 +0 0 0 1 +1 0.5 0.5 0.4 +box +0.20 1.5 1.56 +-0.45 0 0.78 +0 0 0 1 +1 0.5 0.5 0.4 +box +1.1 1.5 0.16 +0 0 1.48 +0 0 0 1 +0.5 0.5 0.5 0.4 +* Wall +1 +box +1.74 0.1 2.05 +0.045 -0.8 0.555 +0 0 0 1 +0.5 0.5 0.5 0.4 +* Save Table +3 +box +1.1 0.24 0.05 +0 0.21 0.025 +0 0 0 1 +1 0.5 0.5 0 +box +1.1 0.24 0.05 +0 -0.21 0.025 +0 0 0 1 +1 0.5 0.5 0 +box +0.47 0.18 0.05 +0.315 0 0.025 +0 0 0 1 +1 0.5 0.5 0 +* Work table +1 +box +1.1 0.66 0.04 +0 0 -0.02 +0 0 0 1 +0.5 0.5 0.5 1 +* Pole +2 +box +0.04 0.04 1.6 +-0.552 0 0.8 +0 0 0 1 +0.5 0.5 0.5 1 +box +1.1 0.04 0.04 +0 0 1.58 +0 0 0 1 +0.5 0.5 0.5 1 +* Drop box +2 +box +0.47 0.37 0.64 +0.315 -0.515 -0.19 +0 0 0 1 +0.231 0.305 0.258 0.5 +box +0.47 0.37 0.64 +0.315 0.515 -0.19 +0 0 0 1 +0.231 0.305 0.258 0.5 +. diff --git a/motoman_perception/CMakeLists.txt b/motoman_perception/CMakeLists.txt new file mode 100644 index 0000000..ad4c48f --- /dev/null +++ b/motoman_perception/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 2.8.3) +project(motoman_perception) + +find_package(catkin REQUIRED COMPONENTS + roscpp + rospy + std_msgs + sensor_msgs + tf2_sensor_msgs +) + +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES motoman_perception +# CATKIN_DEPENDS roscpp rospy std_msg +# DEPENDS system_lib +) + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} +) + diff --git a/motoman_perception/package.xml b/motoman_perception/package.xml new file mode 100644 index 0000000..0ca2f5e --- /dev/null +++ b/motoman_perception/package.xml @@ -0,0 +1,32 @@ + + + motoman_perception + 0.0.0 + The motoman_perception package + + motoman + + TODO + + + catkin + roscpp + rospy + std_msgs + tf2_sensor_msgs + roscpp + rospy + std_msgs + tf2_sensor_msgs + roscpp + rospy + std_msgs + tf2_sensor_msgs + + + + + + + + diff --git a/motoman_perception/script/collision_map_interface.py b/motoman_perception/script/collision_map_interface.py new file mode 100755 index 0000000..ffc4597 --- /dev/null +++ b/motoman_perception/script/collision_map_interface.py @@ -0,0 +1,260 @@ +#! /usr/bin/python +# Software License Agreement (BSD License) +# +## @package collision_map_interface +# Python version of collision_map_interface.cpp +# Functions that take and reset the collision map, and add, remove, attach +# or detach objects + +import random, time, math, scipy, pdb +import rospy +from arm_navigation_msgs.msg import Shape +from tabletop_object_detector.msg import Table +from arm_navigation_msgs.msg import MakeStaticCollisionMapAction, MakeStaticCollisionMapGoal +from arm_navigation_msgs.msg import CollisionObject, AttachedCollisionObject, CollisionObjectOperation +from actionlib_msgs.msg import GoalStatus +from std_srvs.srv import Empty, EmptyRequest +from arm_navigation_msgs.msg import Shape +import actionlib +import tf +from geometry_msgs.msg import PoseStamped, Point, Quaternion, Pose +from arm_navigation_msgs.srv import SetPlanningSceneDiff, SetPlanningSceneDiffRequest + + +#interface to the collision map +class CollisionMapInterface(): + + def __init__(self): + + #set up service clients, publishers, and action clients + rospy.loginfo("advertising on collision object topics") + self.object_in_map_pub = rospy.Publisher("collision_object", CollisionObject) + self.attached_object_pub = rospy.Publisher("attached_collision_object", AttachedCollisionObject) + + rospy.loginfo("waiting for collision map reset service") + rospy.wait_for_service("collider_node/reset") + rospy.loginfo("found collision map reset service") + self.reset_collision_map_srv = rospy.ServiceProxy("collider_node/reset", Empty) + + rospy.loginfo("waiting for set planning scene diff service") + rospy.wait_for_service("environment_server/set_planning_scene_diff") + self.set_planning_scene_srv = rospy.ServiceProxy("environment_server/set_planning_scene_diff", SetPlanningSceneDiff) + + self.object_in_map_current_id = 0 + rospy.loginfo("done initializing collision map interface") + + + ##set the planning scene (so IK and move_arm can run) + def set_planning_scene(self, ordered_collision_ops = None, link_padding = None): + req = SetPlanningSceneDiffRequest() + if ordered_collision_ops != None: + req.operations = ordered_collision_ops + if link_padding != None: + req.planning_scene_diff.link_padding = link_padding + try: + resp = self.set_planning_scene_srv(req) + except: + rospy.logerr("error in calling set_planning_scene_diff!") + return 0 + return 1 + + + ##clear everything in the octomap + def clear_octomap(self): + self.reset_collision_map_srv() + + + ##reset all the current collision objects + def reset_collision_map(self): + + #remove all objects + reset_object = CollisionObject() + reset_object.operation.operation = CollisionObjectOperation.REMOVE + reset_object.header.frame_id = "base_link" + reset_object.header.stamp = rospy.Time.now() + reset_object.id = "all" + self.object_in_map_pub.publish(reset_object) + + #and all attached objects + reset_attached_objects = AttachedCollisionObject() + reset_attached_objects.link_name = "all" + reset_attached_objects.object.header.frame_id = "base_link" + reset_attached_objects.object.header.stamp = rospy.Time.now() + reset_attached_objects.object = reset_object + self.attached_object_pub.publish(reset_attached_objects) + + #and clear the octomap + self.clear_octomap() + + rospy.loginfo("collision objects reset") + self.object_in_map_current_id = 0. + return 1 + + ##attaches an object to the gripper for the purposes of collision-aware + #motion planning + def attach_object_to_gripper(self, object_name, whicharm = 'r'): + + obj = AttachedCollisionObject() + + obj.link_name = whicharm+"_gripper_r_finger_tip_link" + obj.object.operation.operation = CollisionObjectOperation.ATTACH_AND_REMOVE_AS_OBJECT + obj.object.header.stamp = rospy.Time.now() + obj.object.header.frame_id = "base_link" + obj.object.id = object_name + touch_links = ["_gripper_palm_link", "_gripper_r_finger_tip_link", "_gripper_l_finger_tip_link", "_gripper_l_finger_link", "_gripper_r_finger_link"] + obj.touch_links = [whicharm+link for link in touch_links] + self.attached_object_pub.publish(obj) + + + ##detaches all objects from the gripper + #(removes from collision space entirely) + def detach_all_objects_from_gripper(self, whicharm = 'r'): + + rospy.loginfo("detaching all objects from gripper %s"%whicharm) + obj = AttachedCollisionObject() + obj.object.header.stamp = rospy.Time.now() + obj.object.header.frame_id = "base_link" + obj.link_name = whicharm+"_gripper_r_finger_tip_link" + obj.object.operation.operation = CollisionObjectOperation.REMOVE + self.attached_object_pub.publish(obj) + + + ##detaches all objects from the gripper + #(adds back as objects in the collision space where they are now) + #object_collision_name is the original name for the collision object + def detach_and_add_back_objects_attached_to_gripper(self, whicharm = 'r', object_collision_name = None): + + rospy.loginfo("detaching all objects from gripper %s and adding them back to the collision map"%whicharm) + + if object_collision_name == None: + rospy.loginfo("need to specify the object name! Detaching and not adding back object.") + self.detach_all_objects_from_gripper(whicharm) + return + + obj = AttachedCollisionObject() + obj.object.header.stamp = rospy.Time.now() + obj.object.header.frame_id = "base_link" + obj.link_name = whicharm+"_gripper_r_finger_tip_link" + obj.object.id = object_collision_name + obj.object.operation.operation = CollisionObjectOperation.DETACH_AND_ADD_AS_OBJECT + self.attached_object_pub.publish(obj) + + + ##convert a Pose message to a 4x4 scipy matrix + def pose_to_mat(self, pose): + quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] + pos = scipy.matrix([pose.position.x, pose.position.y, pose.position.z]).T + mat = scipy.matrix(tf.transformations.quaternion_matrix(quat)) + mat[0:3, 3] = pos + return mat + + + #convert a 4x4 scipy matrix to a Pose message + def mat_to_pose(self, mat): + pose = Pose() + pose.position.x = mat[0,3] + pose.position.y = mat[1,3] + pose.position.z = mat[2,3] + quat = tf.transformations.quaternion_from_matrix(mat) + pose.orientation.x = quat[0] + pose.orientation.y = quat[1] + pose.orientation.z = quat[2] + pose.orientation.w = quat[3] + return pose + + + #adds the table to the map + def process_collision_geometry_for_table(self, firsttable, additional_tables = []): + + table_object = CollisionObject() + table_object.operation.operation = CollisionObjectOperation.ADD + table_object.header.frame_id = firsttable.pose.header.frame_id + table_object.header.stamp = rospy.Time.now() + + #create a box for each table + for table in [firsttable,]+additional_tables: + object = Shape() + object.type = Shape.BOX; + object.dimensions.append(math.fabs(table.x_max-table.x_min)) + object.dimensions.append(math.fabs(table.y_max-table.y_min)) + object.dimensions.append(0.01) + table_object.shapes.append(object) + + #set the origin of the table object in the middle of the firsttable + table_mat = self.pose_to_mat(firsttable.pose.pose) + table_offset = scipy.matrix([(firsttable.x_min + firsttable.x_max)/2.0, (firsttable.y_min + firsttable.y_max)/2.0, 0.0]).T + table_offset_mat = scipy.matrix(scipy.identity(4)) + table_offset_mat[0:3,3] = table_offset + table_center = table_mat * table_offset_mat + origin_pose = self.mat_to_pose(table_center) + table_object.poses.append(origin_pose) + + table_object.id = "table" + self.object_in_map_pub.publish(table_object) + + + ##return the next unique collision object name + def get_next_object_name(self): + string = "graspable_object_"+str(self.object_in_map_current_id) + self.object_in_map_current_id += 1 + return string + + + ##adds a single box to the map + def add_collision_box(self, box_pose, box_dims, frame_id, collision_name): + + rospy.loginfo("adding box to collision map") + box = CollisionObject() + box.operation.operation = CollisionObjectOperation.ADD + box.header.frame_id = frame_id + box.header.stamp = rospy.Time.now() + shape = Shape() + shape.type = Shape.BOX + shape.dimensions = box_dims + box.shapes.append(shape) + box.poses.append(box_pose) + box.id = collision_name + self.object_in_map_pub.publish(box) + + + ##adds a cluster to the map as a bunch of small boxes + def process_collision_geometry_for_cluster(self, cluster): + + rospy.loginfo("adding cluster with %d points to collision map"%len(cluster.points)) + + many_boxes = CollisionObject() + + many_boxes.operation.operation = CollisionObjectOperation.ADD + many_boxes.header = cluster.header + many_boxes.header.stamp = rospy.Time.now() + num_to_use = int(len(cluster.points)/100.0) + random_indices = range(len(cluster.points)) + scipy.random.shuffle(random_indices) + random_indices = random_indices[0:num_to_use] + for i in range(num_to_use): + shape = Shape() + shape.type = Shape.BOX + shape.dimensions = [.005]*3 + pose = Pose() + pose.position.x = cluster.points[random_indices[i]].x + pose.position.y = cluster.points[random_indices[i]].y + pose.position.z = cluster.points[random_indices[i]].z + pose.orientation = Quaternion(*[0,0,0,1]) + many_boxes.shapes.append(shape) + many_boxes.poses.append(pose) + + collision_name = self.get_next_object_name() + many_boxes.id = collision_name + self.object_in_map_pub.publish(many_boxes) + return collision_name + + + ##remove a single collision object from the map + def remove_collision_object(self, collision_name): + + reset_object = CollisionObject() + reset_object.operation.operation = CollisionObjectOperation.REMOVE + reset_object.header.frame_id = "base_link" + reset_object.header.stamp = rospy.Time.now() + reset_object.id = collision_name + self.object_in_map_pub.publish(reset_object) diff --git a/motoman_recognition/motoman_euclidean_cluster/launch/euclidean_cluster.launch b/motoman_recognition/motoman_euclidean_cluster/launch/euclidean_cluster.launch index bd7dd39..df53f64 100644 --- a/motoman_recognition/motoman_euclidean_cluster/launch/euclidean_cluster.launch +++ b/motoman_recognition/motoman_euclidean_cluster/launch/euclidean_cluster.launch @@ -15,7 +15,7 @@ - + diff --git a/motoman_recognition/motoman_euclidean_cluster/package.xml b/motoman_recognition/motoman_euclidean_cluster/package.xml index 6778e58..fc005e7 100644 --- a/motoman_recognition/motoman_euclidean_cluster/package.xml +++ b/motoman_recognition/motoman_euclidean_cluster/package.xml @@ -20,6 +20,4 @@ pcl_ros pcl_msgs - - diff --git a/motoman_recognition/motoman_euclidean_cluster/src/input.csv b/motoman_recognition/motoman_euclidean_cluster/src/input.csv new file mode 100644 index 0000000..e69de29 diff --git a/motoman_rviz_plugins/src/bounding_box_display_common.h b/motoman_rviz_plugins/src/bounding_box_display_common.h index 303949a..052b456 100644 --- a/motoman_rviz_plugins/src/bounding_box_display_common.h +++ b/motoman_rviz_plugins/src/bounding_box_display_common.h @@ -90,57 +90,61 @@ namespace motoman_rviz_plugins { template - class BoundingBoxDisplayCommon: public rviz::MessageFilterDisplay - { -public: - BoundingBoxDisplayCommon() {}; - ~BoundingBoxDisplayCommon() {}; - typedef boost::shared_ptr ShapePtr; - typedef boost::shared_ptr BillboardLinePtr; - typedef boost::shared_ptr ArrowPtr; - -protected: - QColor color_; - std::string coloring_method_; - double alpha_; - double line_width_; - - std::vector > coords_objects_; - std::vector coords_nodes_; - std::vector edges_; - std::vector shapes_; - - QColor getColor( - size_t index, - const motoman_viz_msgs::BoundingBox& box, - double min_value, - double max_value) + class BoundingBoxDisplayCommon: public rviz::MessageFilterDisplay { - if (coloring_method_ == "auto") { - std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(index); - return QColor(ros_color.r * 255.0, - ros_color.g * 255.0, - ros_color.b * 255.0, - ros_color.a * 255.0); - } - else if (coloring_method_ == "flat") { - return color_; - } - else if (coloring_method_ == "label") { - std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(box.label); - return QColor(ros_color.r * 255.0, - ros_color.g * 255.0, - ros_color.b * 255.0, - ros_color.a * 255.0); - } - else if (coloring_method_ == "value") { - if (min_value != max_value) { - std_msgs::ColorRGBA ros_color = jsk_topic_tools::heatColor((box.value - min_value) / (max_value - min_value)); + public: + BoundingBoxDisplayCommon() {}; + ~BoundingBoxDisplayCommon() {}; + typedef boost::shared_ptr ShapePtr; + typedef boost::shared_ptr BillboardLinePtr; + typedef boost::shared_ptr ArrowPtr; + + protected: + QColor color_; + std::string coloring_method_; + double alpha_; + double line_width_; + + std::vector > coords_objects_; + std::vector coords_nodes_; + std::vector edges_; + std::vector shapes_; + + QColor getColor( + size_t index, + const motoman_viz_msgs::BoundingBox& box, + double min_value, + double max_value) + { + if (coloring_method_ == "auto") { + std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(index); + return QColor(ros_color.r * 255.0, + ros_color.g * 255.0, + ros_color.b * 255.0, + ros_color.a * 255.0); + } + else if (coloring_method_ == "flat") { + return color_; + } + else if (coloring_method_ == "label") { + std_msgs::ColorRGBA ros_color = jsk_topic_tools::colorCategory20(box.label); return QColor(ros_color.r * 255.0, ros_color.g * 255.0, ros_color.b * 255.0, ros_color.a * 255.0); } + else if (coloring_method_ == "value") { + if (min_value != max_value) { + std_msgs::ColorRGBA ros_color = jsk_topic_tools::heatColor((box.value - min_value) / (max_value - min_value)); + return QColor(ros_color.r * 255.0, + ros_color.g * 255.0, + ros_color.b * 255.0, + ros_color.a * 255.0); + } + else { + return QColor(255.0, 255.0, 255.0, 255.0); + } + } else if (coloring_method_ == "tag") { std_msgs::ColorRGBA ros_color; auto itr = tag_dict_.find(box.tag); @@ -155,307 +159,302 @@ namespace motoman_rviz_plugins ros_color.b * 255.0, ros_color.a * 255.0); } - - else { - return QColor(255.0, 255.0, 255.0, 255.0); - } } - } - bool isValidBoundingBox( - const motoman_viz_msgs::BoundingBox box_msg) - { - // Check size - if (box_msg.dimensions.x < 1.0e-9 || - box_msg.dimensions.y < 1.0e-9 || - box_msg.dimensions.z < 1.0e-9 || - std::isnan(box_msg.dimensions.x) || - std::isnan(box_msg.dimensions.y) || - std::isnan(box_msg.dimensions.z)) { - return false; - } - return true; - } - - void allocateShapes(int num) - { - if (num > shapes_.size()) { - for (size_t i = shapes_.size(); i < num; i++) { - ShapePtr shape (new rviz::Shape( - rviz::Shape::Cube, this->context_->getSceneManager(), - this->scene_node_)); - shapes_.push_back(shape); + bool isValidBoundingBox( + const motoman_viz_msgs::BoundingBox box_msg) + { + // Check size + if (box_msg.dimensions.x < 1.0e-9 || + box_msg.dimensions.y < 1.0e-9 || + box_msg.dimensions.z < 1.0e-9 || + std::isnan(box_msg.dimensions.x) || + std::isnan(box_msg.dimensions.y) || + std::isnan(box_msg.dimensions.z)) { + return false; } + return true; } - else if (num < shapes_.size()) + + void allocateShapes(int num) { - shapes_.resize(num); + if (num > shapes_.size()) { + for (size_t i = shapes_.size(); i < num; i++) { + ShapePtr shape (new rviz::Shape( + rviz::Shape::Cube, this->context_->getSceneManager(), + this->scene_node_)); + shapes_.push_back(shape); + } + } + else if (num < shapes_.size()) + { + shapes_.resize(num); + } } - } - void allocateBillboardLines(int num) - { - if (num > edges_.size()) { - for (size_t i = edges_.size(); i < num; i++) { - BillboardLinePtr line(new rviz::BillboardLine( - this->context_->getSceneManager(), this->scene_node_)); - edges_.push_back(line); + void allocateBillboardLines(int num) + { + if (num > edges_.size()) { + for (size_t i = edges_.size(); i < num; i++) { + BillboardLinePtr line(new rviz::BillboardLine( + this->context_->getSceneManager(), this->scene_node_)); + edges_.push_back(line); + } } + else if (num < edges_.size()) + { + edges_.resize(num); // ok?? + } } - else if (num < edges_.size()) - { - edges_.resize(num); // ok?? - } - } - void allocateCoords(int num) - { - if (num > coords_objects_.size()) { - for (size_t i = coords_objects_.size(); i < num; i++) { - Ogre::SceneNode* scene_node = this->scene_node_->createChildSceneNode(); - std::vector coord; - for (int i = 0; i < 3; i++) { - ArrowPtr arrow (new rviz::Arrow(this->scene_manager_, scene_node)); - coord.push_back(arrow); + void allocateCoords(int num) + { + if (num > coords_objects_.size()) { + for (size_t i = coords_objects_.size(); i < num; i++) { + Ogre::SceneNode* scene_node = this->scene_node_->createChildSceneNode(); + std::vector coord; + for (int i = 0; i < 3; i++) { + ArrowPtr arrow (new rviz::Arrow(this->scene_manager_, scene_node)); + coord.push_back(arrow); + } + coords_nodes_.push_back(scene_node); + coords_objects_.push_back(coord); } - coords_nodes_.push_back(scene_node); - coords_objects_.push_back(coord); } - } - else if (num < coords_objects_.size()) { - for (size_t i = num; i < coords_objects_.size(); i++) { - // coords_nodes_[i]; - // coords_objects_[i][0]->setVisible(false); - // coords_objects_[i][1]->setVisible(false); - // coords_objects_[i][2]->setVisible(false); - coords_nodes_[i]->setVisible(false); + else if (num < coords_objects_.size()) { + for (size_t i = num; i < coords_objects_.size(); i++) { + // coords_nodes_[i]; + // coords_objects_[i][0]->setVisible(false); + // coords_objects_[i][1]->setVisible(false); + // coords_objects_[i][2]->setVisible(false); + coords_nodes_[i]->setVisible(false); + } + coords_objects_.resize(num); + coords_nodes_.resize(num); } - coords_objects_.resize(num); - coords_nodes_.resize(num); - } - } - - void showBoxes( - const motoman_viz_msgs::BoundingBoxArray::ConstPtr& msg) - { - edges_.clear(); - allocateShapes(msg->boxes.size()); - float min_value = DBL_MAX; - float max_value = -DBL_MAX; - for (size_t i = 0; i < msg->boxes.size(); i++) { - min_value = std::min(min_value, msg->boxes[i].value); - max_value = std::max(max_value, msg->boxes[i].value); } - for (size_t i = 0; i < msg->boxes.size(); i++) { - motoman_viz_msgs::BoundingBox box = msg->boxes[i]; - if (!isValidBoundingBox(box)) { - ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]", - box.dimensions.x, box.dimensions.y, box.dimensions.z); - continue; - } - ShapePtr shape = shapes_[i]; - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if(!this->context_->getFrameManager()->transform( - box.header, box.pose, position, orientation)) { - ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", - box.header.frame_id.c_str(), qPrintable(this->fixed_frame_)); - return; + void showBoxes( + const motoman_viz_msgs::BoundingBoxArray::ConstPtr& msg) + { + edges_.clear(); + allocateShapes(msg->boxes.size()); + float min_value = DBL_MAX; + float max_value = -DBL_MAX; + for (size_t i = 0; i < msg->boxes.size(); i++) { + min_value = std::min(min_value, msg->boxes[i].value); + max_value = std::max(max_value, msg->boxes[i].value); } + for (size_t i = 0; i < msg->boxes.size(); i++) { + motoman_viz_msgs::BoundingBox box = msg->boxes[i]; + if (!isValidBoundingBox(box)) { + ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]", + box.dimensions.x, box.dimensions.y, box.dimensions.z); + continue; + } - // Ogre::Vector3 p(box.pose.position.x, - // box.pose.position.y, - // box.pose.position.z); - // Ogre::Quaternion q(box.pose.orientation.w, - // box.pose.orientation.x, - // box.pose.orientation.y, - // box.pose.orientation.z); - shape->setPosition(position); - shape->setOrientation(orientation); - Ogre::Vector3 dimensions; - dimensions[0] = box.dimensions.x; - dimensions[1] = box.dimensions.y; - dimensions[2] = box.dimensions.z; - shape->setScale(dimensions); - QColor color = getColor(i, box, min_value, max_value); - shape->setColor(color.red() / 255.0, - color.green() / 255.0, - color.blue() / 255.0, - alpha_); - } - } + ShapePtr shape = shapes_[i]; + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if(!this->context_->getFrameManager()->transform( + box.header, box.pose, position, orientation)) { + ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", + box.header.frame_id.c_str(), qPrintable(this->fixed_frame_)); + return; + } - void showEdges( - const motoman_viz_msgs::BoundingBoxArray::ConstPtr& msg) - { - shapes_.clear(); - allocateBillboardLines(msg->boxes.size()); - float min_value = DBL_MAX; - float max_value = -DBL_MAX; - for (size_t i = 0; i < msg->boxes.size(); i++) { - min_value = std::min(min_value, msg->boxes[i].value); - max_value = std::max(max_value, msg->boxes[i].value); + // Ogre::Vector3 p(box.pose.position.x, + // box.pose.position.y, + // box.pose.position.z); + // Ogre::Quaternion q(box.pose.orientation.w, + // box.pose.orientation.x, + // box.pose.orientation.y, + // box.pose.orientation.z); + shape->setPosition(position); + shape->setOrientation(orientation); + Ogre::Vector3 dimensions; + dimensions[0] = box.dimensions.x; + dimensions[1] = box.dimensions.y; + dimensions[2] = box.dimensions.z; + shape->setScale(dimensions); + QColor color = getColor(i, box, min_value, max_value); + shape->setColor(color.red() / 255.0, + color.green() / 255.0, + color.blue() / 255.0, + alpha_); + } } - for (size_t i = 0; i < msg->boxes.size(); i++) { - motoman_viz_msgs::BoundingBox box = msg->boxes[i]; - if (!isValidBoundingBox(box)) { - ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]", - box.dimensions.x, box.dimensions.y, box.dimensions.z); - continue; + void showEdges( + const motoman_viz_msgs::BoundingBoxArray::ConstPtr& msg) + { + shapes_.clear(); + allocateBillboardLines(msg->boxes.size()); + float min_value = DBL_MAX; + float max_value = -DBL_MAX; + for (size_t i = 0; i < msg->boxes.size(); i++) { + min_value = std::min(min_value, msg->boxes[i].value); + max_value = std::max(max_value, msg->boxes[i].value); } - geometry_msgs::Vector3 dimensions = box.dimensions; - - BillboardLinePtr edge = edges_[i]; - edge->clear(); - Ogre::Vector3 position; - Ogre::Quaternion quaternion; - if(!this->context_->getFrameManager()->transform(box.header, box.pose, - position, - quaternion)) { - ROS_ERROR( "Error transforming pose" - "'%s' from frame '%s' to frame '%s'", - qPrintable( this->getName() ), box.header.frame_id.c_str(), - qPrintable( this->fixed_frame_ )); - return; // return? + for (size_t i = 0; i < msg->boxes.size(); i++) { + motoman_viz_msgs::BoundingBox box = msg->boxes[i]; + if (!isValidBoundingBox(box)) { + ROS_WARN_THROTTLE(10, "Invalid size of bounding box is included and skipped: [%f, %f, %f]", + box.dimensions.x, box.dimensions.y, box.dimensions.z); + continue; + } + + geometry_msgs::Vector3 dimensions = box.dimensions; + + BillboardLinePtr edge = edges_[i]; + edge->clear(); + Ogre::Vector3 position; + Ogre::Quaternion quaternion; + if(!this->context_->getFrameManager()->transform(box.header, box.pose, + position, + quaternion)) { + ROS_ERROR( "Error transforming pose" + "'%s' from frame '%s' to frame '%s'", + qPrintable( this->getName() ), box.header.frame_id.c_str(), + qPrintable( this->fixed_frame_ )); + return; // return? + } + edge->setPosition(position); + edge->setOrientation(quaternion); + + edge->setMaxPointsPerLine(2); + edge->setNumLines(12); + edge->setLineWidth(line_width_); + QColor color = getColor(i, box, min_value, max_value); + edge->setColor(color.red() / 255.0, + color.green() / 255.0, + color.blue() / 255.0, + alpha_); + + Ogre::Vector3 A, B, C, D, E, F, G, H; + A[0] = dimensions.x / 2.0; + A[1] = dimensions.y / 2.0; + A[2] = dimensions.z / 2.0; + B[0] = - dimensions.x / 2.0; + B[1] = dimensions.y / 2.0; + B[2] = dimensions.z / 2.0; + C[0] = - dimensions.x / 2.0; + C[1] = - dimensions.y / 2.0; + C[2] = dimensions.z / 2.0; + D[0] = dimensions.x / 2.0; + D[1] = - dimensions.y / 2.0; + D[2] = dimensions.z / 2.0; + + E[0] = dimensions.x / 2.0; + E[1] = dimensions.y / 2.0; + E[2] = - dimensions.z / 2.0; + F[0] = - dimensions.x / 2.0; + F[1] = dimensions.y / 2.0; + F[2] = - dimensions.z / 2.0; + G[0] = - dimensions.x / 2.0; + G[1] = - dimensions.y / 2.0; + G[2] = - dimensions.z / 2.0; + H[0] = dimensions.x / 2.0; + H[1] = - dimensions.y / 2.0; + H[2] = - dimensions.z / 2.0; + + edge->addPoint(A); edge->addPoint(B); edge->newLine(); + edge->addPoint(B); edge->addPoint(C); edge->newLine(); + edge->addPoint(C); edge->addPoint(D); edge->newLine(); + edge->addPoint(D); edge->addPoint(A); edge->newLine(); + edge->addPoint(E); edge->addPoint(F); edge->newLine(); + edge->addPoint(F); edge->addPoint(G); edge->newLine(); + edge->addPoint(G); edge->addPoint(H); edge->newLine(); + edge->addPoint(H); edge->addPoint(E); edge->newLine(); + edge->addPoint(A); edge->addPoint(E); edge->newLine(); + edge->addPoint(B); edge->addPoint(F); edge->newLine(); + edge->addPoint(C); edge->addPoint(G); edge->newLine(); + edge->addPoint(D); edge->addPoint(H); } - edge->setPosition(position); - edge->setOrientation(quaternion); - - edge->setMaxPointsPerLine(2); - edge->setNumLines(12); - edge->setLineWidth(line_width_); - QColor color = getColor(i, box, min_value, max_value); - edge->setColor(color.red() / 255.0, - color.green() / 255.0, - color.blue() / 255.0, - alpha_); - - Ogre::Vector3 A, B, C, D, E, F, G, H; - A[0] = dimensions.x / 2.0; - A[1] = dimensions.y / 2.0; - A[2] = dimensions.z / 2.0; - B[0] = - dimensions.x / 2.0; - B[1] = dimensions.y / 2.0; - B[2] = dimensions.z / 2.0; - C[0] = - dimensions.x / 2.0; - C[1] = - dimensions.y / 2.0; - C[2] = dimensions.z / 2.0; - D[0] = dimensions.x / 2.0; - D[1] = - dimensions.y / 2.0; - D[2] = dimensions.z / 2.0; - - E[0] = dimensions.x / 2.0; - E[1] = dimensions.y / 2.0; - E[2] = - dimensions.z / 2.0; - F[0] = - dimensions.x / 2.0; - F[1] = dimensions.y / 2.0; - F[2] = - dimensions.z / 2.0; - G[0] = - dimensions.x / 2.0; - G[1] = - dimensions.y / 2.0; - G[2] = - dimensions.z / 2.0; - H[0] = dimensions.x / 2.0; - H[1] = - dimensions.y / 2.0; - H[2] = - dimensions.z / 2.0; - - edge->addPoint(A); edge->addPoint(B); edge->newLine(); - edge->addPoint(B); edge->addPoint(C); edge->newLine(); - edge->addPoint(C); edge->addPoint(D); edge->newLine(); - edge->addPoint(D); edge->addPoint(A); edge->newLine(); - edge->addPoint(E); edge->addPoint(F); edge->newLine(); - edge->addPoint(F); edge->addPoint(G); edge->newLine(); - edge->addPoint(G); edge->addPoint(H); edge->newLine(); - edge->addPoint(H); edge->addPoint(E); edge->newLine(); - edge->addPoint(A); edge->addPoint(E); edge->newLine(); - edge->addPoint(B); edge->addPoint(F); edge->newLine(); - edge->addPoint(C); edge->addPoint(G); edge->newLine(); - edge->addPoint(D); edge->addPoint(H); } - } - void showCoords( - const motoman_viz_msgs::BoundingBoxArray::ConstPtr& msg) - { - allocateCoords(msg->boxes.size()); - for (size_t i = 0; i < msg->boxes.size(); i++) { - motoman_viz_msgs::BoundingBox box = msg->boxes[i]; - std::vector coord = coords_objects_[i]; - - Ogre::SceneNode* scene_node = coords_nodes_[i]; - scene_node->setVisible(true); - Ogre::Vector3 position; - Ogre::Quaternion orientation; - if(!this->context_->getFrameManager()->getTransform( - box.header, position, orientation)) { - ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", - box.header.frame_id.c_str(), qPrintable(this->fixed_frame_)); - return; - } - scene_node->setPosition(position); - scene_node->setOrientation(orientation); // scene node is at frame pose - - float color[3][3] = {{1, 0, 0}, - {0, 1, 0}, - {0, 0, 1}}; - for (int j = 0; j < 3; j++) { - // check radius diraction - Ogre::Vector3 scale; - if (color[j][0] == 1) { - scale = Ogre::Vector3( - box.dimensions.x, - std::min(box.dimensions.y, box.dimensions.z), - std::min(box.dimensions.y, box.dimensions.z)); - } - if (color[j][1] == 1) { - scale = Ogre::Vector3( - box.dimensions.y, - std::min(box.dimensions.x, box.dimensions.z), - std::min(box.dimensions.x, box.dimensions.z)); + void showCoords( + const motoman_viz_msgs::BoundingBoxArray::ConstPtr& msg) + { + allocateCoords(msg->boxes.size()); + for (size_t i = 0; i < msg->boxes.size(); i++) { + motoman_viz_msgs::BoundingBox box = msg->boxes[i]; + std::vector coord = coords_objects_[i]; + + Ogre::SceneNode* scene_node = coords_nodes_[i]; + scene_node->setVisible(true); + Ogre::Vector3 position; + Ogre::Quaternion orientation; + if(!this->context_->getFrameManager()->getTransform( + box.header, position, orientation)) { + ROS_DEBUG("Error transforming from frame '%s' to frame '%s'", + box.header.frame_id.c_str(), qPrintable(this->fixed_frame_)); + return; } - if (color[j][2] == 1) { - scale = Ogre::Vector3( - box.dimensions.z, - std::min(box.dimensions.x, box.dimensions.y), - std::min(box.dimensions.x, box.dimensions.y)); + scene_node->setPosition(position); + scene_node->setOrientation(orientation); // scene node is at frame pose + + float color[3][3] = {{1, 0, 0}, + {0, 1, 0}, + {0, 0, 1}}; + for (int j = 0; j < 3; j++) { + // check radius diraction + Ogre::Vector3 scale; + if (color[j][0] == 1) { + scale = Ogre::Vector3( + box.dimensions.x, + std::min(box.dimensions.y, box.dimensions.z), + std::min(box.dimensions.y, box.dimensions.z)); + } + if (color[j][1] == 1) { + scale = Ogre::Vector3( + box.dimensions.y, + std::min(box.dimensions.x, box.dimensions.z), + std::min(box.dimensions.x, box.dimensions.z)); + } + if (color[j][2] == 1) { + scale = Ogre::Vector3( + box.dimensions.z, + std::min(box.dimensions.x, box.dimensions.y), + std::min(box.dimensions.x, box.dimensions.y)); + } + + Ogre::Vector3 direction(color[j][0], color[j][1], color[j][2]); + Ogre::Vector3 pos(box.pose.position.x, + box.pose.position.y, + box.pose.position.z); + Ogre::Quaternion qua(box.pose.orientation.w, + box.pose.orientation.x, + box.pose.orientation.y, + box.pose.orientation.z); + direction = qua * direction; + Ogre::ColourValue rgba; + rgba.a = 1; + rgba.r = color[j][0]; + rgba.g = color[j][1]; + rgba.b = color[j][2]; + + ArrowPtr arrow = coords_objects_[i][j]; + arrow->setPosition(pos); + arrow->setDirection(direction); + arrow->setScale(scale); + arrow->setColor(rgba); } - - Ogre::Vector3 direction(color[j][0], color[j][1], color[j][2]); - Ogre::Vector3 pos(box.pose.position.x, - box.pose.position.y, - box.pose.position.z); - Ogre::Quaternion qua(box.pose.orientation.w, - box.pose.orientation.x, - box.pose.orientation.y, - box.pose.orientation.z); - direction = qua * direction; - Ogre::ColourValue rgba; - rgba.a = 1; - rgba.r = color[j][0]; - rgba.g = color[j][1]; - rgba.b = color[j][2]; - - ArrowPtr arrow = coords_objects_[i][j]; - arrow->setPosition(pos); - arrow->setDirection(direction); - arrow->setScale(scale); - arrow->setColor(rgba); } } - } - void hideCoords() - { - for (size_t i = 0; i < coords_nodes_.size(); i++) { - coords_nodes_[i]->setVisible(false); + void hideCoords() + { + for (size_t i = 0; i < coords_nodes_.size(); i++) { + coords_nodes_[i]->setVisible(false); + } } - } - private: - std::map tag_dict_; + private: + std::map tag_dict_; - }; // class BoundingBoxDisplayCommon + }; // class BoundingBoxDisplayCommon } // namespace motoman_rviz_plugins