diff --git a/ros_essentials/simple_arm/CMakeLists.txt b/ros_essentials/simple_arm/CMakeLists.txt
index 2471dbd..89c74b4 100644
--- a/ros_essentials/simple_arm/CMakeLists.txt
+++ b/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)
diff --git a/ros_essentials/simple_arm/launch/robot_spawn.launch b/ros_essentials/simple_arm/launch/robot_spawn.launch
index 4e124ae..915046d 100644
--- a/ros_essentials/simple_arm/launch/robot_spawn.launch
+++ b/ros_essentials/simple_arm/launch/robot_spawn.launch
@@ -17,13 +17,16 @@
+
+
+
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
diff --git a/ros_essentials/simple_arm/src/look_away.cpp b/ros_essentials/simple_arm/src/look_away.cpp
new file mode 100644
index 0000000..edcdf5a
--- /dev/null
+++ b/ros_essentials/simple_arm/src/look_away.cpp
@@ -0,0 +1,82 @@
+#include "ros/ros.h"
+#include "simple_arm/GoToPosition.h"
+#include
+#include
+
+// Define global vector of joints last position, moving state of the arm, and the client that can request services
+std::vector 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 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("/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;
+}