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; +}