Created sub-pub node with service to do something based on the curren…

…t arm's image

ros_essentials/simple_arm/CMakeLists.txt

@@ -200,3 +200,7 @@ add_dependencies(simple_mover simple_arm_generate_messages_cpp)
 add_executable(arm_mover src/arm_mover.cpp)
 target_link_libraries(arm_mover ${catkin_LIBRARIES})
 add_dependencies(arm_mover simple_arm_generate_messages_cpp)
+
+add_executable(look_away src/look_away.cpp)
+target_link_libraries(look_away ${catkin_LIBRARIES})
+add_dependencies(look_away simple_arm_generate_messages_cpp)

ros_essentials/simple_arm/launch/robot_spawn.launch

@@ -17,13 +17,16 @@
   <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
   args="-urdf -param robot_description -x 0 -y 0 -z 0 -R 0 -P 0 -Y 0 -model simple_arm"/>
 
+  <!-- The look away node -->
+  <node name="look_away" type="look_away" pkg="simple_arm"/>
+
   <!-- The arm mover node -->
   <node name="arm_mover" type="arm_mover" pkg="simple_arm" output="screen">
     <rosparam>
       min_joint_1_angle: 0
       max_joint_1_angle: 1.57
       min_joint_2_angle: 0
-      max_joint_2_angle: 1.0
+      max_joint_2_angle: 1.57
     </rosparam>
   </node>
 

ros_essentials/simple_arm/src/look_away.cpp

@@ -0,0 +1,82 @@
+#include "ros/ros.h"
+#include "simple_arm/GoToPosition.h"
+#include <sensor_msgs/JointState.h>
+#include <sensor_msgs/Image.h>
+
+// Define global vector of joints last position, moving state of the arm, and the client that can request services
+std::vector<double> joints_last_position{ 0, 0 };
+bool moving_state = false;
+ros::ServiceClient client;
+
+// This function calls the safe_move service to safely move the arm to the center position
+void move_arm_center()
+{
+    ROS_INFO_STREAM("Moving the arm to the center");
+
+    // Request centered joint angles [1.57, 1.57]
+    simple_arm::GoToPosition srv;
+    srv.request.joint_1 = 1.57;
+    srv.request.joint_2 = 1.57;
+
+    // Call the safe_move service and pass the requested joint angles
+    if (!client.call(srv))
+        ROS_ERROR("Failed to call service safe_move");
+}
+
+// This callback function continuously executes and reads the arm joint angles position
+void joint_states_callback(const sensor_msgs::JointState js)
+{
+    // Get joints current position
+    std::vector<double> joints_current_position = js.position;
+
+    // Define a tolerance threshold to compare double values
+    double tolerance = 0.0005;
+
+    // Check if the arm is moving by comparing its current joints position to its latest
+    if (fabs(joints_current_position[0] - joints_last_position[0]) < tolerance && fabs(joints_current_position[1] - joints_last_position[1]) < tolerance)
+        moving_state = false;
+    else {
+        moving_state = true;
+        joints_last_position = joints_current_position;
+    }
+}
+
+// This callback function continuously executes and reads the image data
+void look_away_callback(const sensor_msgs::Image img)
+{
+
+    bool uniform_image = true;
+
+    // Loop through each pixel in the image and check if its equal to the first one
+    for (int i = 0; i < img.height * img.step; i++) {
+        if (img.data[i] - img.data[0] != 0) {
+            uniform_image = false;
+            break;
+        }
+    }
+
+    // If the image is uniform and the arm is not moving, move the arm to the center
+    if (uniform_image == true && moving_state == false)
+        move_arm_center();
+}
+
+int main(int argc, char** argv)
+{
+    // Initialize the look_away node and create a handle to it
+    ros::init(argc, argv, "look_away");
+    ros::NodeHandle n;
+
+    // Define a client service capable of requesting services from safe_move
+    client = n.serviceClient<simple_arm::GoToPosition>("/arm_mover/safe_move");
+
+    // Subscribe to /simple_arm/joint_states topic to read the arm joints position inside the joint_states_callback function
+    ros::Subscriber sub1 = n.subscribe("/simple_arm/joint_states", 10, joint_states_callback);
+
+    // Subscribe to rgb_camera/image_raw topic to read the image data inside the look_away_callback function
+    ros::Subscriber sub2 = n.subscribe("rgb_camera/image_raw", 10, look_away_callback);
+
+    // Handle ROS communication events
+    ros::spin();
+
+    return 0;
+}