diff --git a/.gitignore b/.gitignore index cc0b46d..cc21741 100644 --- a/.gitignore +++ b/.gitignore @@ -1,9 +1,9 @@ # Don't track the content of these folders # ############################################ -rrrobot_ws/lib/ -rrrobot_ws/build -rrrobot_ws/devel -rrrobot_ws/install +src/*/lib/ +src/*/build +src/*/devel +src/*/install .* !.gitignore diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..380eeba --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "src/ariac_ws/src/universal_robot"] + path = src/ariac_ws/src/universal_robot + url = https://github.com/osrf/universal_robot + branch = ariac_2019_ur10_moveit_config diff --git a/docker_env/docker-compose.yml b/docker_env/docker-compose.yml index 0343d22..0d620fe 100644 --- a/docker_env/docker-compose.yml +++ b/docker_env/docker-compose.yml @@ -60,7 +60,7 @@ services: # Mount gazebo folder on host to app folder in container volumes: #- .docker_env/gazebo/.gazebo:/root/.gazebo - - ../rrrobot_ws:/app/rrrobot_ws + - ../src/rrrobot_ws:/app/rrrobot_ws # Set DISPLAY variable and network mode for GUIs environment: - DISPLAY=${IP_ADDRESS}:0.0 @@ -77,12 +77,13 @@ services: build: ./gear # Mount gear folder on host to app folder in container volumes: - - ../rrrobot_ws:/app/rrrobot_ws + - ../src/rrrobot_ws:/app/rrrobot_ws + - ../src/ariac_ws:/app/ariac_ws # Set DISPLAY variable and network mode for GUIs environment: - DISPLAY=${IP_ADDRESS}:0.0 # Set working directory in container to app folder - working_dir: /app/rrrobot_ws + working_dir: /app hostname: "rrrobot-env" networks: - ros diff --git a/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp b/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp deleted file mode 100644 index 524855f..0000000 --- a/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// depth_camera_node.cpp - -#include -#include -#include - -#include -#include "sensor_msgs/PointCloud.h" -#include "sensor_msgs/PointCloud2.h" -#include "message_filters/subscriber.h" -#include "pcl/io/pcd_io.h" -#include "pcl/point_types.h" -#include -#include -#include -#include - -#include -#include -#include - - -pcl::PointCloud convert_pc(const sensor_msgs::PointCloud::ConstPtr & msg) { - // Convert from sensor_msgs/PointCloud.msg to necessary format - // Convert to PointCloud2 - sensor_msgs::PointCloud2 cloud2 = new sensor_msgs::PointCloud2; - sensor_msgs::PointCloudToPointCloud2(msg, cloud2); - - // Convert to PCL data type - pcl::PointCloud output = new pcl::PointCloud; - pcl::fromROSMsg(cloud2, output); -} - -void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & msg) { - pcl::PointCloud cloud = convert_pc(msg); - - // extract clusters - -} - -int main(int argc, char ** argv) { - // TODO: Subscribe to depth camera topic - // Last argument is the default name of the node. - ros::init(argc, argv, "depth_camera_node"); - - ros::NodeHandle node; - - ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback); - - - - // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item - - - // TODO: Publish object's current location to topic for RRRobot node to listen to - -} \ No newline at end of file diff --git a/rrrobot_ws/src/CMakeLists.txt b/src/ariac_ws/src/CMakeLists.txt similarity index 100% rename from rrrobot_ws/src/CMakeLists.txt rename to src/ariac_ws/src/CMakeLists.txt diff --git a/src/ariac_ws/src/universal_robot b/src/ariac_ws/src/universal_robot new file mode 160000 index 0000000..8bb711d --- /dev/null +++ b/src/ariac_ws/src/universal_robot @@ -0,0 +1 @@ +Subproject commit 8bb711dc9f81087306ae962f9e251e69dc31721d diff --git a/rrrobot_ws/build.sh b/src/rrrobot_ws/build.sh similarity index 100% rename from rrrobot_ws/build.sh rename to src/rrrobot_ws/build.sh diff --git a/src/rrrobot_ws/src/CMakeLists.txt b/src/rrrobot_ws/src/CMakeLists.txt new file mode 100644 index 0000000..e5cafb2 --- /dev/null +++ b/src/rrrobot_ws/src/CMakeLists.txt @@ -0,0 +1,67 @@ +# toplevel CMakeLists.txt for a catkin workspace +# catkin/cmake/toplevel.cmake + +cmake_minimum_required(VERSION 3.0.2) + +set(CATKIN_TOPLEVEL TRUE) + +# search for catkin within the workspace +set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}") +execute_process(COMMAND ${_cmd} + RESULT_VARIABLE _res + OUTPUT_VARIABLE _out + ERROR_VARIABLE _err + OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_STRIP_TRAILING_WHITESPACE +) +if(NOT _res EQUAL 0 AND NOT _res EQUAL 2) + # searching fot catkin resulted in an error + string(REPLACE ";" " " _cmd_str "${_cmd}") + message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}") +endif() + +# include catkin from workspace or via find_package() +if(_res EQUAL 0) + set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake") + # include all.cmake without add_subdirectory to let it operate in same scope + include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE) + add_subdirectory("${_out}") + +else() + # use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument + # or CMAKE_PREFIX_PATH from the environment + if(NOT DEFINED CMAKE_PREFIX_PATH) + if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "") + if(NOT WIN32) + string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + else() + set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH}) + endif() + endif() + endif() + + # list of catkin workspaces + set(catkin_search_path "") + foreach(path ${CMAKE_PREFIX_PATH}) + if(EXISTS "${path}/.catkin") + list(FIND catkin_search_path ${path} _index) + if(_index EQUAL -1) + list(APPEND catkin_search_path ${path}) + endif() + endif() + endforeach() + + # search for catkin in all workspaces + set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE) + find_package(catkin QUIET + NO_POLICY_SCOPE + PATHS ${catkin_search_path} + NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH) + unset(CATKIN_TOPLEVEL_FIND_PACKAGE) + + if(NOT catkin_FOUND) + message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.") + endif() +endif() + +catkin_workspace() diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/arm_joint_and_link_definitions.jpg b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/arm_joint_and_link_definitions.jpg similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/arm_joint_and_link_definitions.jpg rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/arm_joint_and_link_definitions.jpg diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_1.STL b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_1.STL similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_1.STL rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_1.STL diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_2.STL b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_2.STL similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_2.STL rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_2.STL diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_3.STL b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_3.STL similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_3.STL rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_3.STL diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_4.STL b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_4.STL similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_4.STL rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_4.STL diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_5.STL b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_5.STL similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_5.STL rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_5.STL diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_6.STL b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_6.STL similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_6.STL rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/arm_link_6.STL diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/base_link.STL b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/base_link.STL similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/base_link.STL rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/meshes/base_link.STL diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.config b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.config similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.config rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.config diff --git a/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.sdf b/src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.sdf similarity index 100% rename from rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.sdf rename to src/rrrobot_ws/src/gazebo_models/fanuc_robotic_arm_with_gripper/model.sdf diff --git a/rrrobot_ws/src/gazebo_models/model_mappings.txt b/src/rrrobot_ws/src/gazebo_models/model_mappings.txt similarity index 100% rename from rrrobot_ws/src/gazebo_models/model_mappings.txt rename to src/rrrobot_ws/src/gazebo_models/model_mappings.txt diff --git a/rrrobot_ws/src/gazebo_models/plastic_bottle/images/plastic-bottle-1.jpg b/src/rrrobot_ws/src/gazebo_models/plastic_bottle/images/plastic-bottle-1.jpg similarity index 100% rename from rrrobot_ws/src/gazebo_models/plastic_bottle/images/plastic-bottle-1.jpg rename to src/rrrobot_ws/src/gazebo_models/plastic_bottle/images/plastic-bottle-1.jpg diff --git a/rrrobot_ws/src/gazebo_models/plastic_bottle/meshes/plastic_bottle.STL b/src/rrrobot_ws/src/gazebo_models/plastic_bottle/meshes/plastic_bottle.STL similarity index 100% rename from rrrobot_ws/src/gazebo_models/plastic_bottle/meshes/plastic_bottle.STL rename to src/rrrobot_ws/src/gazebo_models/plastic_bottle/meshes/plastic_bottle.STL diff --git a/rrrobot_ws/src/gazebo_models/plastic_bottle/model.config b/src/rrrobot_ws/src/gazebo_models/plastic_bottle/model.config similarity index 100% rename from rrrobot_ws/src/gazebo_models/plastic_bottle/model.config rename to src/rrrobot_ws/src/gazebo_models/plastic_bottle/model.config diff --git a/rrrobot_ws/src/gazebo_models/plastic_bottle/model.sdf b/src/rrrobot_ws/src/gazebo_models/plastic_bottle/model.sdf similarity index 100% rename from rrrobot_ws/src/gazebo_models/plastic_bottle/model.sdf rename to src/rrrobot_ws/src/gazebo_models/plastic_bottle/model.sdf diff --git a/rrrobot_ws/src/gazebo_models/square_trash_can/model.config b/src/rrrobot_ws/src/gazebo_models/square_trash_can/model.config similarity index 100% rename from rrrobot_ws/src/gazebo_models/square_trash_can/model.config rename to src/rrrobot_ws/src/gazebo_models/square_trash_can/model.config diff --git a/rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf b/src/rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf similarity index 100% rename from rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf rename to src/rrrobot_ws/src/gazebo_models/square_trash_can/model.sdf diff --git a/rrrobot_ws/src/gazebo_models/unit_box/image.png b/src/rrrobot_ws/src/gazebo_models/unit_box/image.png similarity index 100% rename from rrrobot_ws/src/gazebo_models/unit_box/image.png rename to src/rrrobot_ws/src/gazebo_models/unit_box/image.png diff --git a/rrrobot_ws/src/gazebo_models/unit_box/model.config b/src/rrrobot_ws/src/gazebo_models/unit_box/model.config similarity index 100% rename from rrrobot_ws/src/gazebo_models/unit_box/model.config rename to src/rrrobot_ws/src/gazebo_models/unit_box/model.config diff --git a/rrrobot_ws/src/gazebo_models/unit_box/model.sdf b/src/rrrobot_ws/src/gazebo_models/unit_box/model.sdf similarity index 100% rename from rrrobot_ws/src/gazebo_models/unit_box/model.sdf rename to src/rrrobot_ws/src/gazebo_models/unit_box/model.sdf diff --git a/rrrobot_ws/src/rrrobot/CMakeLists.txt b/src/rrrobot_ws/src/rrrobot/CMakeLists.txt similarity index 100% rename from rrrobot_ws/src/rrrobot/CMakeLists.txt rename to src/rrrobot_ws/src/rrrobot/CMakeLists.txt diff --git a/rrrobot_ws/src/rrrobot/config/rrrobot.yaml b/src/rrrobot_ws/src/rrrobot/config/rrrobot.yaml similarity index 100% rename from rrrobot_ws/src/rrrobot/config/rrrobot.yaml rename to src/rrrobot_ws/src/rrrobot/config/rrrobot.yaml diff --git a/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml b/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml similarity index 76% rename from rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml rename to src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml index d3976e9..aa94c75 100644 --- a/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml +++ b/src/rrrobot_ws/src/rrrobot/config/rrrobot_sensors.yaml @@ -7,5 +7,5 @@ sensors: depth_camera_1: type: depth_camera pose: - xyz: [1.2, 0.75, 1.5] - rpy: [0, 1.5707, 0] + xyz: [0.75, 0.75, 1.5] + rpy: [0, 1, 0] diff --git a/rrrobot_ws/src/rrrobot/launch/rrrobot.launch b/src/rrrobot_ws/src/rrrobot/launch/rrrobot.launch similarity index 100% rename from rrrobot_ws/src/rrrobot/launch/rrrobot.launch rename to src/rrrobot_ws/src/rrrobot/launch/rrrobot.launch diff --git a/rrrobot_ws/src/rrrobot/msg/model_insertion.msg b/src/rrrobot_ws/src/rrrobot/msg/model_insertion.msg similarity index 100% rename from rrrobot_ws/src/rrrobot/msg/model_insertion.msg rename to src/rrrobot_ws/src/rrrobot/msg/model_insertion.msg diff --git a/rrrobot_ws/src/rrrobot/package.xml b/src/rrrobot_ws/src/rrrobot/package.xml similarity index 100% rename from rrrobot_ws/src/rrrobot/package.xml rename to src/rrrobot_ws/src/rrrobot/package.xml diff --git a/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh b/src/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh old mode 100755 new mode 100644 similarity index 100% rename from rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh rename to src/rrrobot_ws/src/rrrobot/scripts/rrrobot_run.sh diff --git a/rrrobot_ws/src/rrrobot/scripts/rrrobot_run_no_build.sh b/src/rrrobot_ws/src/rrrobot/scripts/rrrobot_run_no_build.sh old mode 100755 new mode 100644 similarity index 100% rename from rrrobot_ws/src/rrrobot/scripts/rrrobot_run_no_build.sh rename to src/rrrobot_ws/src/rrrobot/scripts/rrrobot_run_no_build.sh diff --git a/rrrobot_ws/src/rrrobot/scripts/sample_run.sh b/src/rrrobot_ws/src/rrrobot/scripts/sample_run.sh old mode 100755 new mode 100644 similarity index 100% rename from rrrobot_ws/src/rrrobot/scripts/sample_run.sh rename to src/rrrobot_ws/src/rrrobot/scripts/sample_run.sh diff --git a/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp similarity index 89% rename from rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp rename to src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp index 9feb701..700c618 100644 --- a/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -10,6 +10,17 @@ #include #include +#include +#include + +#include +#include + +#include +#include + +#include + Class ArmController { public: diff --git a/rrrobot_ws/src/rrrobot/src/cv_model.py b/src/rrrobot_ws/src/rrrobot/src/cv_model.py similarity index 100% rename from rrrobot_ws/src/rrrobot/src/cv_model.py rename to src/rrrobot_ws/src/rrrobot/src/cv_model.py diff --git a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp new file mode 100644 index 0000000..8862759 --- /dev/null +++ b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp @@ -0,0 +1,139 @@ +// depth_camera_node.cpp + +#include +#include +#include + +#include +#include "sensor_msgs/PointCloud.h" +#include "sensor_msgs/PointCloud2.h" +#include "message_filters/subscriber.h" +#include "pcl/io/pcd_io.h" +#include "pcl/point_types.h" +#include +#include +#include +#include + +#include +#include +#include + + +void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & cloud_msg) { + // convert from PointCloud to PointCloud2 + sensor_msgs::PointCloud2 cloud2; + sensor_msgs::convertPointCloudToPointCloud2(*cloud_msg, cloud2); + + // Container for original & filtered data + pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; + pcl::PCLPointCloud2ConstPtr cloudPtr(cloud); + pcl::PCLPointCloud2 cloud_filtered; + + // Convert to PCL data type + pcl_conversions::toPCL(cloud2, *cloud); + + // Perform the actual filtering + pcl::VoxelGrid sor; + sor.setInputCloud (cloudPtr); + sor.setLeafSize (0.02, 0.02, 0.02); + sor.filter (cloud_filtered); + + pcl::PointCloud point_cloud; + pcl::PointCloud::Ptr point_cloudPtr(new pcl::PointCloud); + pcl::fromPCLPointCloud2( cloud_filtered, point_cloud); + pcl::copyPointCloud(point_cloud, *point_cloudPtr); + + // Create the KdTree object for the search method of the extraction + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(point_cloudPtr); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(0.1); // 2cm + ec.setMinClusterSize(10); //100 + ec.setMaxClusterSize(99000000); + ec.setSearchMethod(tree); + ec.setInputCloud(point_cloudPtr); + ec.extract(cluster_indices); + + pcl::PointCloud::Ptr point_cloud_segmented(new pcl::PointCloud); + + int j= 0; + + for (std::vector::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it) + { + for (std::vector::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit) + { + pcl::PointXYZRGB point; + point.x = point_cloudPtr->points[*pit].x; + point.y = point_cloudPtr->points[*pit].y; + point.z = point_cloudPtr->points[*pit].z; + + if (j == 0) //Red #FF0000 (255,0,0) + { + point.r = 0; + point.g = 0; + point.b = 255; + } + else if (j == 1) //Lime #00FF00 (0,255,0) + { + point.r = 0; + point.g = 255; + point.b = 0; + } + else if (j == 2) // Blue #0000FF (0,0,255) + { + point.r = 255; + point.g = 0; + point.b = 0; + } + else + { + if (j % 2 == 0) + { + point.r = 255 * j / (cluster_indices.size()); + point.g = 128; + point.b = 50; + } + else + { + point.r = 0; + point.g = 255 * j / (cluster_indices.size()); + point.b = 128; + } + } + point_cloud_segmented->push_back(point); + + } + j++; + } + + // Convert to ROS data type + point_cloud_segmented->header.frame_id = point_cloudPtr->header.frame_id; + if(point_cloud_segmented->size()) pcl::toPCLPointCloud2(*point_cloud_segmented, cloud_filtered); + else pcl::toPCLPointCloud2(*point_cloudPtr, cloud_filtered); + sensor_msgs::PointCloud2 output; + pcl_conversions::fromPCL(cloud_filtered, output); + + // Publish the data + pub.publish (output); +} + +int main(int argc, char ** argv) { + // TODO: Subscribe to depth camera topic + // Last argument is the default name of the node. + ros::init(argc, argv, "depth_camera_node"); + + ros::NodeHandle node; + + ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback); + + + + // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item + + + // TODO: Publish object's current location to topic for RRRobot node to listen to + +} \ No newline at end of file diff --git a/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp b/src/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp similarity index 100% rename from rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp rename to src/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp diff --git a/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp b/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp similarity index 100% rename from rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp rename to src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp diff --git a/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp similarity index 100% rename from rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp rename to src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp diff --git a/rrrobot_ws/src/rrrobot/src/v1_0.001.pt b/src/rrrobot_ws/src/rrrobot/src/v1_0.001.pt similarity index 100% rename from rrrobot_ws/src/rrrobot/src/v1_0.001.pt rename to src/rrrobot_ws/src/rrrobot/src/v1_0.001.pt diff --git a/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp b/src/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp similarity index 100% rename from rrrobot_ws/src/rrrobot/test/test_insert_object.cpp rename to src/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp diff --git a/rrrobot_ws/world/gear.py b/src/rrrobot_ws/world/gear.py old mode 100755 new mode 100644 similarity index 100% rename from rrrobot_ws/world/gear.py rename to src/rrrobot_ws/world/gear.py diff --git a/rrrobot_ws/world/gear.world.template b/src/rrrobot_ws/world/gear.world.template similarity index 100% rename from rrrobot_ws/world/gear.world.template rename to src/rrrobot_ws/world/gear.world.template diff --git a/rrrobot_ws/world/rrrobot_setpoint.world b/src/rrrobot_ws/world/rrrobot_setpoint.world similarity index 100% rename from rrrobot_ws/world/rrrobot_setpoint.world rename to src/rrrobot_ws/world/rrrobot_setpoint.world