Makes depth camera publish to the channel the rrrobot_node is listening on

This commit is contained in:
Derek Witcpalek
2020-04-26 10:46:23 -04:00
parent 4098662c5d
commit 85d0bff6ec
2 changed files with 14 additions and 1 deletions

11
src/rrrobot_ws/run_all.sh Executable file
View File

@@ -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 &

View File

@@ -21,6 +21,8 @@
#include <std_msgs/String.h> #include <std_msgs/String.h>
#include <std_srvs/Trigger.h> #include <std_srvs/Trigger.h>
#include "topic_names.h"
ros::Publisher pub; ros::Publisher pub;
void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) 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); ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
pub = node.advertise<geometry_msgs::Pose>("output", 1); pub = node.advertise<geometry_msgs::Pose>(DESIRED_GRASP_POSE_CHANNEL, 1);
ros::spin(); 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 // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item