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();