diff --git a/air_moveit_config/config/air_config-2.yaml b/air_moveit_config/config/air_config-2.yaml new file mode 100644 index 0000000..f022d11 --- /dev/null +++ b/air_moveit_config/config/air_config-2.yaml @@ -0,0 +1,47 @@ +# Total planning attempts +max_solutions: 10 + + +# Planning group and link names +arm_group_name: "air" +eef_name: "gripper" +hand_group_name: "gripper" +hand_frame: "qbhand_base_link" + +# Poses +hand_open_pose: "open" +hand_close_pose: "full_closed" +arm_home_pose: "home2" + +# Scene frames +world_frame: "world" +table_reference_frame: "world" +object_reference_frame: "world" +surface_link: "table" + +# Collision object for picking +# CYLINDER object specifications +object_name: "49" +object_dimensions: [0.05, 0.01] # [height, radius] +object_pose: [0.55, -0.25, 0.7, 0, 0, 0] + +# Table model +spawn_table: true +table_name: "table" +table_dimensions: [2, 2, 0.001] # [length, width, height] +table_pose: [0.2, -0.85, 0.65, 0, 0, 0] + +# Gripper grasp frame transform [x,y,z,r,p,y] +grasp_frame_transform: [-0.08, 0.124, 0.12, 1.571, 0, 0] + +# Place pose [x,y,z,r,p,y] +place_pose: [0.12, -0.7, 0.8, 0, 0, 0] +place_surface_offset: 0.0005 # place offset from table + +# Valid distance range when approaching an object for picking +approach_object_min_dist: 0.01 +approach_object_max_dist: 0.1 + +# Valid createObjectheight range when lifting an object after pick +lift_object_min_dist: 0.01 +lift_object_max_dist: 0.1 diff --git a/air_moveit_config/config/air_config-3.yaml b/air_moveit_config/config/air_config-3.yaml new file mode 100644 index 0000000..c1af02c --- /dev/null +++ b/air_moveit_config/config/air_config-3.yaml @@ -0,0 +1,49 @@ +# Total planning attempts +max_solutions: 10 + + +# Planning group and link names +arm_group_name: "air" +eef_name: "gripper" +hand_group_name: "gripper" +hand_frame: "qbhand_base_link" + +# Poses +hand_open_pose: "open" +hand_close_pose: "closed" +arm_home_pose: "home2" + +# Scene frames +world_frame: "world" +table_reference_frame: "world" +object_reference_frame: "world" +surface_link: "table" + +# Collision object for picking +# CYLINDER object specifications +object_name: "47" +object_dimensions: [0.05, 0.01] # [height, radius] +object_pose: [0.55, -0.25, 0.7, 0, 0, 0] + +# Table model +# Table model +spawn_table: true +table_name: "table" +table_dimensions: [2, 2, 0.001] # [length, width, height] +table_pose: [0.2, -0.85, 0.63, 0, 0, 0] + +# Gripper grasp frame transform [x,y,z,r,p,y] +grasp_frame_transform: [-0.05, 0.12, 0.12, 1.571, 0, 0] + +# Place pose [x,y,z,r,p,y] +place_pose: [0.42, -0.77, 0.8, 0, 0, 0] +place_surface_offset: 0.0005 # place offset from table + +# Valid distance range when approaching an object for picking +approach_object_min_dist: 0.01 +approach_object_max_dist: 0.1 + +# Valid createObjectheight range when lifting an object after pick +lift_object_min_dist: 0.001 +lift_object_max_dist: 0.1 + diff --git a/air_moveit_config/config/air_config.yaml b/air_moveit_config/config/air_config.yaml index c878a9f..a40f31d 100644 --- a/air_moveit_config/config/air_config.yaml +++ b/air_moveit_config/config/air_config.yaml @@ -1,5 +1,5 @@ # Total planning attempts -max_solutions: 1000 +max_solutions: 10 # Planning group and link names @@ -11,37 +11,37 @@ hand_frame: "qbhand_base_link" # Poses hand_open_pose: "open" hand_close_pose: "full_closed" -arm_home_pose: "up" +arm_home_pose: "home2" # Scene frames world_frame: "world" table_reference_frame: "world" object_reference_frame: "world" -surface_link: "table" +surface_link: "base_link" # Collision object for picking # CYLINDER object specifications -object_name: "sphere" +object_name: "80" object_dimensions: [0.05, 0.01] # [height, radius] -object_pose: [0.55, -0.25, 0.7, 0, 0, 0] +object_pose: [0.55, -0.25, 0.61, 0, 0, 0] # Table model -spawn_table: True +spawn_table: true table_name: "table" -table_dimensions: [0.4, 0.5, 0.2] # [length, width, height] -table_pose: [0.4, -0.21, 0.69, 0, 0, 0] +table_dimensions: [2, 2, 0.001] # [length, width, height] +table_pose: [0.2, -0.85, 0.65, 0, 0, 0] # Gripper grasp frame transform [x,y,z,r,p,y] -grasp_frame_transform: [0.0, 0.07, 0.13, 1.571, 0, 0] +grasp_frame_transform: [-0.08, 0.15, 0.12, 1.571, 0, 0] # Place pose [x,y,z,r,p,y] -place_pose: [0.59, -0.25, 0.82, 0, 0, 0] +place_pose: [0.12, -0.86, 0.8, 0, 0, 0] place_surface_offset: 0.0005 # place offset from table # Valid distance range when approaching an object for picking -approach_object_min_dist: 0.00 -approach_object_max_dist: 0.25 +approach_object_min_dist: 0.01 +approach_object_max_dist: 0.1 -# Valid height range when lifting an object after pick -lift_object_min_dist: 0.00 -lift_object_max_dist: 0.15 +# Valid createObjectheight range when lifting an object after pick +lift_object_min_dist: 0.02 +lift_object_max_dist: 0.1 diff --git a/air_moveit_config/config/joint_limits.yaml b/air_moveit_config/config/joint_limits.yaml index 1f7b37c..4099995 100644 --- a/air_moveit_config/config/joint_limits.yaml +++ b/air_moveit_config/config/joint_limits.yaml @@ -2,214 +2,214 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 1.0 +default_acceleration_scaling_factor: 1.0 # 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: ej: has_velocity_limits: true - max_velocity: 0.13 + max_velocity: 0.35 has_acceleration_limits: false max_acceleration: 0 pitchj: has_velocity_limits: true - max_velocity: 0.09 + max_velocity: 0.3 has_acceleration_limits: false max_acceleration: 0 qbhand_index_distal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_index_distal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_index_knuckle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_index_middle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_index_middle_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_index_proximal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_index_proximal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_little_distal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_little_distal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_little_knuckle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_little_middle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_little_middle_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_little_proximal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_little_proximal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_middle_distal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_middle_distal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_middle_knuckle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_middle_middle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_middle_middle_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_middle_proximal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_middle_proximal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_ring_distal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_ring_distal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_ring_knuckle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_ring_middle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_ring_middle_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_ring_proximal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_ring_proximal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_synergy_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_thumb_distal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_thumb_distal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_thumb_knuckle_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_thumb_proximal_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 qbhand_thumb_proximal_virtual_joint: has_velocity_limits: true - max_velocity: 100 + max_velocity: 0.5 has_acceleration_limits: false max_acceleration: 0 rollj: has_velocity_limits: true - max_velocity: 0.09 + max_velocity: 0.3 has_acceleration_limits: false max_acceleration: 0 s1j: has_velocity_limits: true - max_velocity: 0.35 + max_velocity: 0.4 has_acceleration_limits: false max_acceleration: 0 s2j: has_velocity_limits: true - max_velocity: 0.35 + max_velocity: 0.4 has_acceleration_limits: false max_acceleration: 0 s3j: has_velocity_limits: true - max_velocity: 0.35 + max_velocity: 0.4 has_acceleration_limits: false max_acceleration: 0 yawj: has_velocity_limits: true - max_velocity: 1.75 + max_velocity: 0.4 has_acceleration_limits: false max_acceleration: 0 \ No newline at end of file diff --git a/air_moveit_config/config/mtc.rviz b/air_moveit_config/config/mtc.rviz index c937804..69b979c 100644 --- a/air_moveit_config/config/mtc.rviz +++ b/air_moveit_config/config/mtc.rviz @@ -4,9 +4,10 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Motion Planning Tasks1 + - /MotionPlanning1 + - /MotionPlanning1/Planning Request1 Splitter Ratio: 0.5393258333206177 - Tree Height: 135 + Tree Height: 144 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -23,17 +24,17 @@ Panels: Name: Motion Planning Tasks Tasks View: property_splitter: - - 539 + - 232 - 0 solution_sorting: column: 1 order: 0 solutions_splitter: - - 306 - - 98 + - 523 + - 168 solutions_view_columns: ~ tasks_view_columns: - - 133 + - 350 - 57 - 57 - 57 @@ -825,8 +826,8 @@ Visualization Manager: Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: gripper - Query Goal State: true + Planning Group: air + Query Goal State: false Query Start State: false Show Workspace: false Start State Alpha: 1 @@ -1069,6 +1070,46 @@ Visualization Manager: Show Robot Visual: true Value: true Velocity_Scaling_Factor: 1 + - 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: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Min Color: 0; 0; 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /camera/depth/color/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /yolov7/yolov7/visualization + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -1085,7 +1126,7 @@ Visualization Manager: Views: Current: Class: rviz/XYOrbit - Distance: 1.9497904777526855 + Distance: 2.3318021297454834 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1093,26 +1134,28 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.30880630016326904 - Y: -0.1259305477142334 - Z: 1.3560062370743253e-06 + X: 0.7346615195274353 + Y: -0.6179076433181763 + Z: 1.1175876579727628e-06 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.4747973680496216 + Pitch: 0.5097970366477966 Target Frame: base_link - Yaw: 5.124948501586914 + Yaw: 2.219952344894409 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 768 + Height: 1016 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false + Image: + collapsed: false Motion Planning Tasks: collapsed: false Motion Planning Tasks - Slider: @@ -1121,9 +1164,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 Views: collapsed: false - Width: 1621 - X: 249 + Width: 1874 + X: 46 Y: 27 diff --git a/air_moveit_config/include/detect_and_add.hpp b/air_moveit_config/include/detect_and_add.hpp index f7056d8..17adfdd 100644 --- a/air_moveit_config/include/detect_and_add.hpp +++ b/air_moveit_config/include/detect_and_add.hpp @@ -5,7 +5,12 @@ #include #include #include +#include #include +#include + +#include + class air_object { public: @@ -14,8 +19,11 @@ class air_object void yolo_callback(const vision_msgs::Detection2DArrayConstPtr& d2d_arr_msg); void pcl_callback(const sensor_msgs::PointCloud2ConstPtr& msg); - void radius_estimator_callback(const std_msgs::Float32ConstPtr& radius); - void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr& xyz); + void radius_estimator_callback(const std_msgs::Float32MultiArray::ConstPtr& radius); + void sphereCallback(const vision_msgs::Detection3DArrayConstPtr& detections); + //void sphereCallback(const std_msgs::Float32MultiArray::ConstPtr& xyz); + + pcl::PointXYZ getPointXYZ(int x, int y); std_msgs::Float32MultiArray m_xyz; ros::NodeHandle m_NodeHandle; @@ -26,6 +34,7 @@ class air_object ros::Subscriber m_sub; ros::Subscriber m_estimator_sub; + sensor_msgs::PointCloud2 m_pcl_msg; float m_x; float m_y; float m_z; @@ -34,9 +43,12 @@ class air_object int m_center_y; int m_center_x_buffer; int m_center_y_buffer; - float m_radius; + std::vector m_radius; + private: + const std::vector m_RegisteredObjIDs = {32, 40}; + }; #endif // DETECT_AND_ADD_HPP \ No newline at end of file diff --git a/air_moveit_config/launch/air_bringup.launch b/air_moveit_config/launch/air_bringup.launch index 8ddebf7..3ca74ce 100644 --- a/air_moveit_config/launch/air_bringup.launch +++ b/air_moveit_config/launch/air_bringup.launch @@ -19,7 +19,7 @@ - + diff --git a/air_moveit_config/launch/pickplace-2.launch b/air_moveit_config/launch/pickplace-2.launch new file mode 100644 index 0000000..8250e24 --- /dev/null +++ b/air_moveit_config/launch/pickplace-2.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/air_moveit_config/launch/pickplace-3.launch b/air_moveit_config/launch/pickplace-3.launch new file mode 100644 index 0000000..213a641 --- /dev/null +++ b/air_moveit_config/launch/pickplace-3.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/air_moveit_config/src/add_sphere.cpp b/air_moveit_config/src/add_sphere.cpp index a4b8883..e83f017 100644 --- a/air_moveit_config/src/add_sphere.cpp +++ b/air_moveit_config/src/add_sphere.cpp @@ -51,14 +51,14 @@ void air_object::sphereCallback(const vision_msgs::Detection3DArrayConstPtr& det ROS_INFO("Radius: %f", radius); geometry_msgs::Pose pose; - float cam_degree = 30; // Angle between robot and camera + float cam_degree = 42; // Angle between robot and camera pose.position.x = z + (((radius/2) / sin(((90-cam_degree) * M_PI)/180))*2); pose.position.y = -x; pose.position.z = -y + (radius/4); //hotfix for collision // Orientation fix (to do: cam_degree) - pose.orientation.w = 0.9659258; - pose.orientation.y = -0.258819; + pose.orientation.w = 0.9238794463501697; + pose.orientation.y = -0.38268364037636077; moveit_msgs::CollisionObject collision_object; collision_object.header.frame_id = "camera_link"; diff --git a/air_moveit_config/src/pick_place_demo.cpp b/air_moveit_config/src/pick_place_demo.cpp index a03f3cd..bc6608f 100644 --- a/air_moveit_config/src/pick_place_demo.cpp +++ b/air_moveit_config/src/pick_place_demo.cpp @@ -50,7 +50,7 @@ int main(int argc, char** argv) { ros::AsyncSpinner spinner(1); spinner.start(); - //moveit_task_constructor_demo::setupDemoScene(pnh); + moveit_task_constructor_demo::setupDemoScene(pnh); // Construct and run pick/place task moveit_task_constructor_demo::PickPlaceTask pick_place_task("pick_place_task", pnh); @@ -67,7 +67,9 @@ int main(int argc, char** argv) { if (pnh.param("execute", false)) { pick_place_task.execute(); ROS_INFO_NAMED(LOGNAME, "Execution complete"); - system("rosrun air_moveit_config add_sphere"); + //system("rosrun air_moveit_config add_sphere"); + //system("roslaunch air_moveit_config pickplace-2.launch"); + } else { ROS_INFO_NAMED(LOGNAME, "Execution disabled"); system("rosrun air_moveit_config add_sphere"); diff --git a/air_moveit_config/src/pick_place_task.cpp b/air_moveit_config/src/pick_place_task.cpp index 5526cf0..f7228ac 100644 --- a/air_moveit_config/src/pick_place_task.cpp +++ b/air_moveit_config/src/pick_place_task.cpp @@ -97,7 +97,7 @@ void setupDemoScene(ros::NodeHandle& pnh) { moveit::planning_interface::PlanningSceneInterface psi; if (pnh.param("spawn_table", true)) spawnObject(psi, createTable(pnh)); - spawnObject(psi, createObject(pnh)); + //spawnObject(psi, createObject(pnh)); } PickPlaceTask::PickPlaceTask(const std::string& task_name, const ros::NodeHandle& pnh) @@ -240,14 +240,15 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("approach object", cartesian_planner); stage->properties().set("marker_ns", "approach_object"); - stage->properties().set("link", hand_frame_); + stage->setIKFrame(hand_frame_); + //stage->properties().set("link", hand_frame_); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); stage->setMinMaxDistance(approach_object_min_dist_, approach_object_max_dist_); // Set hand forward direction geometry_msgs::Vector3Stamped vec; - vec.header.frame_id = hand_frame_; - vec.vector.y = 1.0; + vec.header.frame_id = world_frame_; + vec.vector.z = -1.0; stage->setDirection(vec); grasp->insert(std::move(stage)); } diff --git a/moveit_task_constructor/demo/src/pick_place_task.cpp b/moveit_task_constructor/demo/src/pick_place_task.cpp index 5eb72ec..e55f57b 100644 --- a/moveit_task_constructor/demo/src/pick_place_task.cpp +++ b/moveit_task_constructor/demo/src/pick_place_task.cpp @@ -401,7 +401,7 @@ bool PickPlaceTask::init() { geometry_msgs::PoseStamped p; p.header.frame_id = object_reference_frame_; p.pose = place_pose_; - p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; + //p.pose.position.z += 0.5 * object_dimensions_[0] + place_surface_offset_; stage->setPose(p); stage->setMonitoredStage(pick_stage_ptr); // hook into successful pick solutions @@ -448,12 +448,12 @@ bool PickPlaceTask::init() { { auto stage = std::make_unique("retreat after place", cartesian_planner); stage->properties().configureInitFrom(Stage::PARENT, { "group" }); - stage->setMinMaxDistance(.12, .25); + stage->setMinMaxDistance(.05, .25); stage->setIKFrame(hand_frame_); stage->properties().set("marker_ns", "retreat"); geometry_msgs::Vector3Stamped vec; vec.header.frame_id = hand_frame_; - vec.vector.z = -1.0; + vec.vector.z = -0.2; stage->setDirection(vec); place->insert(std::move(stage)); } diff --git a/radius_estimation/scripts/dataset_generator.py b/radius_estimation/scripts/dataset_generator.py index bc3283a..555b9ff 100755 --- a/radius_estimation/scripts/dataset_generator.py +++ b/radius_estimation/scripts/dataset_generator.py @@ -21,9 +21,9 @@ def __init__(self): os.makedirs(self.dataset_path) def detection_callback(self, detection_array): - radius = 3.1 + radius = 3.4 for detection in detection_array.detections: - if detection.results[0].id == 32: # nesne ID'si 33 ise + if detection.results[0].id == 32 or detection.results[0].id == 47 : # nesne ID'si 33 ise print("detected") bbox = detection.bbox # Derinlik verisi mevcutsa @@ -53,3 +53,4 @@ def depth_callback(self, depth_image): rospy.init_node('dataset_creator') dataset_creator = DatasetCreator() rospy.spin() +data = data.reshape(1, -1) # add an extra dimension \ No newline at end of file diff --git a/radius_estimation/scripts/predict.py b/radius_estimation/scripts/predict.py index 0d3fe4f..8b8f3e8 100644 --- a/radius_estimation/scripts/predict.py +++ b/radius_estimation/scripts/predict.py @@ -1,7 +1,7 @@ # Modeli kaydetme import joblib # Kaydedilen modeli yükleme -loaded_model = joblib.load("regression_model.joblib") +loaded_model = joblib.load("/home/cenk/regression_model2.joblib") # Gerçek zamanlı kullanım distance = 0.366 diff --git a/realsense-ros/realsense2_camera/launch/rs_camera.launch b/realsense-ros/realsense2_camera/launch/rs_camera.launch index b04fb46..d16a01d 100644 --- a/realsense-ros/realsense2_camera/launch/rs_camera.launch +++ b/realsense-ros/realsense2_camera/launch/rs_camera.launch @@ -3,7 +3,7 @@ - + @@ -35,12 +35,12 @@ - - - - - - + + + + + +