diff --git a/src/rrrobot_ws/run_all.sh b/src/rrrobot_ws/run_all.sh new file mode 100755 index 0000000..d557fff --- /dev/null +++ b/src/rrrobot_ws/run_all.sh @@ -0,0 +1,11 @@ +#/app/rrrobot_ws/src/rrrobot/scripts/rrrobot_run_no_build.sh & +cd /app/rrrobot_ws/src/rrrobot/src +python3 cv_model.py & +cd /app/rrrobot_ws/devel/lib/rrrobot +./rrrobot_node & +./depth_camera_node >> /dev/null & +./object_spawner_node & +./arm_controller_node & +rostopic echo /arm_controller/destination & +rostopic echo /desired_grasp_pose & +rostopic echo /cv_model & diff --git a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp index 8b5b470..812cd89 100644 --- a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp @@ -21,6 +21,8 @@ #include #include +#include "topic_names.h" + ros::Publisher pub; void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) @@ -277,7 +279,7 @@ int main(int argc, char **argv) ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback); - pub = node.advertise("output", 1); + pub = node.advertise(DESIRED_GRASP_POSE_CHANNEL, 1); ros::spin(); // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item