Initialization and Global Variables

In this exercise, we will take a first look at the main application pick_and_place_node.cpp, its public variables, and how to properly initialize it as a ROS node.

Application Variables

Open the file pick_and_place_utilities.h in the following directory:

[workspace source directory]/pick_and_place_application/include/pick_and_place_application/

The C++ class PickAndPlaceConfig defines public global variables used in various parts of the program. These variables are listed below:

ARM_GROUP_NAME = "manipulator";
TCP_LINK_NAME = "tcp_frame";
MARKER_TOPIC = "pick_and_place_marker";
PLANNING_SCENE_TOPIC = "planning_scene";
GET_TARGET_POSE_SERVICE = "get_target_pose";
MOTION_PLAN_SERVICE = "plan_kinematic_path";
ATTACHED_OBJECT_LINK_NAME = "attached_object_link";
WORLD_FRAME_ID = "world_frame";
HOME_POSE_NAME = "home";
WAIT_POSE_NAME = "wait";
AR_TAG_FRAME_ID = "ar_frame";
GRASP_ACTION_NAME = "do_grasp";
BOX_SIZE = tf2::Vector3(0.1f, 0.1f, 0.1f);
BOX_PLACE_TF = tf2::Transform(tf2::Quaternion::getIdentity(), tf2::Vector3(-0.8f, -0.2f, BOX_SIZE.getZ()));
TOUCH_LINKS = std::vector<std::string>();
RETREAT_DISTANCE = 0.05f;
APPROACH_DISTANCE = 0.05f;

PLANNER_ID = "RRTConnect";
PLANNING_ATTEMPTS = 4;
PLANNING_TIME = 20.0;  // seconds

In the main program pick_and_place_node.cpp, the global application object provides access to the program variables through its cfg member.

For instance, in order to use the WORLD_FRAME_ID global variable we would do the following:

ROS_INFO_STREAM("world frame: " << application.cfg.WORLD_FRAME_ID)

Node Initialization

In the pick_and_place_node.cpp file, the following block of code in the main function initializes the PickAndPlace application class and its main ROS components.

int main(int argc, char** argv)
{
  geometry_msgs::msg::Pose box_pose;
  std::vector<geometry_msgs::msg::Pose> pick_poses, place_poses;

  /* =========================================================================================*/
  /*	INITIALIZING ROS NODE
      Goal:
      - Observe all steps needed to properly initialize a ros node.
      - Take notice of how the node's spin() method is invoked from within a thread.  This avoids locking the application.
  /* =========================================================================================*/

  RCLCPP_INFO(LOGGER, "Initialize node");
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);  // This enables loading undeclared parameters
  rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("pick_and_place_node", "", node_options);

  // spinning node in a separate thread
  std::thread spin_thread([node]() {
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
  });

  // creating pick and place application instance
  PickAndPlaceApp application(node);

  // initializing application
  application.initialize();