—Routine Look Away
-
Description of Look Away
To see ROS subscribers and clients in action, write a node called look_away. The look_away node subscribes to the /rgb_camera/image_raw topic, which contains image data from a camera mounted at the end of the robotic arm. Whenever the camera is pointed at an image that is not of interest (in this case, a uniform color image), the callback function will request the safe_move service to safely move the robotic arm to the image of interest. There are a few extra parts of the code to make sure this goes smoothly, we’ll focus on that later.
Update startup files
Just like you did for the arm_mover node, to make look_away launch with other nodes, you need to modify robot_spawn.launch, which can be found in /home/workspace/catkin_ws/src/simple_arm/Launch. Add the following code there:
<!-- The look away node --> <nodename="look_away"type="look_away"pkg="simple_arm"/>
Remember that half a turn of the joint requires pi/2 radians of rotation. Numerically, pi/2 is about 1.57. Since we want the joint to rotate halfway with one request, setting max_joint_2_angle: 1.57 in arm_mover can be helpful:
<!-- The arm mover node -->
<nodename="arm_mover" type="arm_mover" pkg="simple_arm">
<rosparam>
min_joint_1_angle: 0
max_joint_1_angle: 1.57
min_joint_2_angle: 0
max_joint_2_angle: 1.57
</rosparam>
</node>
2.Look Away code
Create an empty look_away node script
The steps taken to create the look_away node are exactly the same as the steps taken to create the simple_mover and arm_mover scripts, but of course changing the actual name of the file itself.
Open a new terminal and enter the following command:
$ cd /home/workspace/catkin_ws/src/simple_arm/src/ $ gedit look_away.cpp
Created and opened the c++ look_away file using gedit editor, now copy and paste the code below and save the file.
look_away.cpp
#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 & amp; & amp; 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 & amp; & amp; 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;
}
code explanation
#include"ros/ros.h" #include"simple_arm/GoToPosition.h" #include <sensor_msgs/JointState.h> #include <sensor_msgs/Image.h>
The header file is similar to the one in arm_mover, only this time we include the JointState.h header file so that we can read the positions of the arm joints. We also include the Image.h header file so that we can use the camera data.
ros::init(argc, argv, "look_away"); ros::NodeHandle n;
Inside the c++ main function, the look_away node is initialized and a ROS NodeHandle object n is instantiated to communicate with ROS.
client = n.serviceClient<simple_arm::GoToPosition>("/arm_mover/safe_move");
Here a client object is created. This node can request the GoToPosition service from the /arm_mover/safe_move service in the previously created arm_mover node. This client object is defined globally in the code, so services can be requested from any function. In particular, this happens in the move_arm_center() function.
ros::Subscriber sub1 = n.subscribe("/simple_arm/joint_states", 10, joint_states_callback);
The first subscriber object sub1 subscribes to the /simple_arm/joint_states topic. By subscribing to this topic, we can track the position of the robotic arm by reading the angles of each joint. queue_size is set to 10, which means a maximum of 10 messages can be stored in the queue. Data from each new incoming message is passed to the joint_states_callback function.
ros::Subscriber sub2 = n.subscribe("rgb_camera/image_raw", 10, look_away_callback);
The second subscription object sub2 subscribes to the /rgb_camera/image_raw topic. queue_size is also set to 10. The look_away_callback function is called every time a new message arrives.
ros::spin();
The ros::spin() function simply blocks until the node receives a shutdown request.
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 & amp; & amp; fabs(joints_current_position[1] - joints_last_position[1]) < tolerance)
moving_state = false;
else {
moving_state = true;
joints_last_position = joints_current_position;
}
}
When sub1 receives a message on the /simple_arm/joint_states topic, the message is passed to the joint_states_callback in the variable js. The joint_states_callback() function checks if the current joint state provided in js is the same as the joint state previously stored in the global joints_last_position variable. If the current and previous joint states are the same (depending on the specified error tolerance), then the arm has stopped moving and the moving_state flag is set to False. The flag is defined globally to be shared with other functions in the code. On the other hand, if the current and previous joint states are different, then the arm is still moving. In this example, the function sets moving_state to true and updates the joints_Last_position variable with the current position data stored in joints_current_position.
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 & amp; & amp; moving_state == false)
move_arm_center();
}
The role of the look_away_callback() function is to receive image data from the topic /rgb_camera/image_raw (http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/Image.html). The callback function (callback) first checks whether all the color values in the image are the same as the color value of the first pixel. If the image is consistent and the arm is not moving, call the move_arm_center() function.
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");
}
In the move_arm_center function, use the arm_mover/safe_move service to create and send a GoToPosition request message to move the two joint angles to 1.57 radians.
3.Look Away: Build, Launch and Interact
Modify CMakeLists.txt
Compiler directives must be included before compiling the look_away.cpp code. As a reminder, for every c++ ROS node written, its dependencies must be added in the CMakeLists.txt file. Open the cmakelsts.txt file for the simple_arm package, located in /home/workspace/catkin_ws/src/simple_arm/, and add the following instructions at the bottom of the file:
add_executable(look_away src/look_away.cpp)
target_link_libraries(look_away ${catkin_LIBRARIES})
add_dependencies(look_away simple_arm_generate_messages_cpp)
build pack
Now that the look_away c++ script has been written and compiler specific instructions included, let’s build the package:
$ cd /home/workspace/catkin_ws/ $ catkin_make
start node
Simple_arm can now be started and interacted with as before:
$ cd /home/workspace/catkin_ws/ $ source devel/setup.bash $ roslaunch simple_arm robot_spawn.launch
Interact with the robotic arm
After starting, the robotic arm should move away from the gray sky and look at the blocks. To view the camera image stream, you can use the same command as before:
$ rqt_image_view/rgb_camera/image_raw
To check that everything is working, open a new terminal and send a service call pointing the robotic arm directly at the sky (note that the newline in the message is necessary):
$ cd /home/workspace/catkin_ws/ $ source devel/setup.bash $ rosservice call /arm_mover/safe_move "joint_1: 0 joint_2: 0"
look_away GitHub branch
A copy of this experiment including all three nodes can be downloaded by visiting the GitHub repo (https://github.com/udacity/RoboND-simple_arm/).
4. Publish-subscribe class
In the Publisher and Subscriber nodes of this article, global variables and objects are defined for use anywhere in the code. This is done to simplify the code, but is not a good practice. In fact, you should always write a publish-subscribe class to easily share variables and objects with any callback functions in your code. Here is a ROS publish-subscribe template class that can be used:
ROS Class C++ code
#include<ros/ros.h>
class SubscribeAndPublish
{
public:
SubscribeAndPublish()
{
//Topic you want to publish
pub_ = n_.advertise<PUBLISHED_MESSAGE_TYPE>("/published_topic", 1);
//Topic you want to subscribe
sub_ = n_.subscribe("/subscribed_topic", 1, &SubscribeAndPublish::callback, this);
}
voidcallback(const SUBSCRIBED_MESSAGE_TYPE & input){
PUBLISHED_MESSAGE_TYPE output;
//.... do something with the input and generate the output...
pub_.publish(output);
}
private:
ros::NodeHandle n_;
ros::Publisher pub_;
ros::Subscriber sub_;
};//End of class SubscribeAndPublish
int main(int argc, char **argv)
{
//Initiate ROS
ros::init(argc, argv, "subscribe_and_publish");
//Create an object of class SubscribeAndPublish that will take care of everything
SubscribeAndPublish SAPObject;
ros::spin();
return0;
}
In subsequent articles, we will use this template class to implement related nodes.