In ROS, node is like a person in this game. It’s a part of the system that knows how to do something, like control a robot’s wheel or take a picture. ROS master acts like a switchboard operator, hence nodes communicate through ROS master.
A topic is like the stories, pictures, or sound effects in the game. It’s a way that nodes share what they know with each other. When a node shares (or publishes) information to a topic, any other node that’s listening (or subscribing) to that topic can use that information to do its job.
Services are similar to topics in that they facilitate the passing of messages in between nodes.
When we say source the environment, like typing source /opt/ros/noetic/setup.bash (where noetic is the version of ROS you’re using), you set up environment variable and paths by running the setup.bash file. he first line of a Bash script often begins with #!/bin/bash or #!/usr/bin/env bash. This line is called a shebang, and it tells the system to interpret the script commands with the Bash shell.
What is Catkin Package? ROS software is organized and distributed into packages, which are directories that might contain source code for ROS nodes, libraries, datasets, and more. Each package also contains a file with build instructions – the CMakeLists.txt file – and a package.xml file with information about the package. Packages enable ROS users to organize useful functionality in a convenient and reusable format.
Building the simple_arm package
Write ROS nodes in C++: simple_mover, arm_mover and look_away
how ROS Publishers work in C++? ros::Publisher pub1 = n.advertise(“/topic_name”, queue_size);
$ cd /home/workspace/catkin_ws/src/simple_arm/src/ $ touch simple_mover.cpp; $ cd /home/workspace/catkin_ws/src/simple_arm/src/ $ gedit simple_mover.cpp
```cpp
#include "ros/ros.h"
#include "std_msgs/Float64.h"
int main(int argc, char** argv)
{
// Initialize the arm_mover node
ros::init(argc, argv, "arm_mover");
// Create a handle to the arm_mover node
ros::NodeHandle n;
// Create a publisher that can publish a std_msgs::Float64 message on the /simple_arm/joint_1_position_controller/command topic
ros::Publisher joint1_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_1_position_controller/command", 10);
// Create a publisher that can publish a std_msgs::Float64 message on the /simple_arm/joint_2_position_controller/command topic
ros::Publisher joint2_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_2_position_controller/command", 10);
// Set loop frequency of 10Hz
ros::Rate loop_rate(10);
int start_time, elapsed;
// Get ROS start time
while (not start_time) {
start_time = ros::Time::now().toSec();
}
while (ros::ok()) {
// Get ROS elapsed time
elapsed = ros::Time::now().toSec() - start_time;
// Set the arm joint angles
std_msgs::Float64 joint1_angle, joint2_angle;
joint1_angle.data = sin(2 *M_PI* 0.1 *elapsed)* (M_PI / 2);
joint2_angle.data = sin(2 *M_PI* 0.1 *elapsed)* (M_PI / 2);
// Publish the arm joint angles
joint1_pub.publish(joint1_angle);
joint2_pub.publish(joint2_angle);
// Sleep for the time remaining until 10 Hz is reached
loop_rate.sleep();
}
return 0;
}
Before you can run the simple_mover node, you have to compile the C++ script.
below is the arm_mover cpp file
#include "ros/ros.h"
#include "simple_arm/GoToPosition.h"
#include <std_msgs/Float64.h>
// Global joint publisher variables
ros::Publisher joint1_pub, joint2_pub;
// This function checks and clamps the joint angles to a safe zone
std::vector<float> clamp_at_boundaries(float requested_j1, float requested_j2)
{
// Define clamped joint angles and assign them to the requested ones
float clamped_j1 = requested_j1;
float clamped_j2 = requested_j2;
// Get min and max joint parameters, and assigning them to their respective variables
float min_j1, max_j1, min_j2, max_j2;
// Assign a new node handle since we have no access to the main one
ros::NodeHandle n2;
// Get node name
std::string node_name = ros::this_node::getName();
// Get joints min and max parameters
n2.getParam(node_name + "/min_joint_1_angle", min_j1);
n2.getParam(node_name + "/max_joint_1_angle", max_j1);
n2.getParam(node_name + "/min_joint_2_angle", min_j2);
n2.getParam(node_name + "/max_joint_2_angle", max_j2);
// Check if joint 1 falls in the safe zone, otherwise clamp it
if (requested_j1 < min_j1 || requested_j1 > max_j1) {
clamped_j1 = std::min(std::max(requested_j1, min_j1), max_j1);
ROS_WARN("j1 is out of bounds, valid range (%1.2f,%1.2f), clamping to: %1.2f", min_j1, max_j1, clamped_j1);
}
// Check if joint 2 falls in the safe zone, otherwise clamp it
if (requested_j2 < min_j2 || requested_j2 > max_j2) {
clamped_j2 = std::min(std::max(requested_j2, min_j2), max_j2);
ROS_WARN("j2 is out of bounds, valid range (%1.2f,%1.2f), clamping to: %1.2f", min_j2, max_j2, clamped_j2);
}
// Store clamped joint angles in a clamped_data vector
std::vector<float> clamped_data = { clamped_j1, clamped_j2 };
return clamped_data;
}
// This callback function executes whenever a safe_move service is requested
bool handle_safe_move_request(simple_arm::GoToPosition::Request& req,
simple_arm::GoToPosition::Response& res)
{
ROS_INFO("GoToPositionRequest received - j1:%1.2f, j2:%1.2f", (float)req.joint_1, (float)req.joint_2);
// Check if requested joint angles are in the safe zone, otherwise clamp them
std::vector<float> joints_angles = clamp_at_boundaries(req.joint_1, req.joint_2);
// Publish clamped joint angles to the arm
std_msgs::Float64 joint1_angle, joint2_angle;
joint1_angle.data = joints_angles[0];
joint2_angle.data = joints_angles[1];
joint1_pub.publish(joint1_angle);
joint2_pub.publish(joint2_angle);
// Wait 3 seconds for arm to settle
ros::Duration(3).sleep();
// Return a response message
res.msg_feedback = "Joint angles set - j1: " + std::to_string(joints_angles[0]) + " , j2: " + std::to_string(joints_angles[1]);
ROS_INFO_STREAM(res.msg_feedback);
return true;
}
int main(int argc, char** argv)
{
// Initialize the arm_mover node and create a handle to it
ros::init(argc, argv, "arm_mover");
ros::NodeHandle n;
// Define two publishers to publish std_msgs::Float64 messages on joints respective topics
joint1_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_1_position_controller/command", 10);
joint2_pub = n.advertise<std_msgs::Float64>("/simple_arm/joint_2_position_controller/command", 10);
// Define a safe_move service with a handle_safe_move_request callback function
ros::ServiceServer service = n.advertiseService("/arm_mover/safe_move", handle_safe_move_request);
ROS_INFO("Ready to send joint commands");
// Handle ROS communication events
ros::spin();
return 0;
}
the next step is to build it, launch it, and test it out via the command line!
next, the look_away cpp file
#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;
}
```
Naixian Zhang 