Converted the executive simple move tool to ros2

management/executive/tools/simple_move.cc

@@ -16,68 +16,73 @@
  * under the License.
  */
 
-#include <ros/ros.h>
-
-#include <ff_msgs/CommandArg.h>
-#include <ff_msgs/CommandConstants.h>
-#include <ff_msgs/CommandStamped.h>
-#include <ff_msgs/AckCompletedStatus.h>
-#include <ff_msgs/AckStamped.h>
-#include <ff_msgs/AckStatus.h>
 #include <ff_common/ff_names.h>
+#include <ff_common/ff_ros.h>
+
+#include <ff_msgs/msg/command_arg.hpp>
+#include <ff_msgs/msg/command_constants.hpp>
+#include <ff_msgs/msg/command_stamped.hpp>
+#include <ff_msgs/msg/ack_completed_status.hpp>
+#include <ff_msgs/msg/ack_stamped.hpp>
+#include <ff_msgs/msg/ack_status.hpp>
 
 #include <string>
 
+FF_DEFINE_LOGGER("simple_move")
+
 std::string unique_cmd_id;
 
-void AckCallback(ff_msgs::AckStampedConstPtr const& Ack) {
+void AckCallback(ff_msgs::msg::AckStamped::SharedPtr const Ack) {
   // Check if the ack corresponds to the command we sent
   if (Ack->cmd_id  == unique_cmd_id) {
     // Check if command hasn't completed
-    if (Ack->completed_status.status == ff_msgs::AckCompletedStatus::NOT) {
-      ROS_INFO("Move command is being executed and has not completed.");
+    if (Ack->completed_status.status == ff_msgs::msg::AckCompletedStatus::NOT) {
+      FF_INFO("Move command is being executed and has not completed.");
       return;  // Return so we don't shut down prematurely
-    } else if (Ack->completed_status.status == ff_msgs::AckCompletedStatus::OK) {
+    } else if (Ack->completed_status.status ==
+                                        ff_msgs::msg:::AckCompletedStatus::OK) {
       // Command completed successfully
-      ROS_INFO("Move command completed successfully!");
+      FF_INFO("Move command completed successfully!");
     } else {  // Command failed
-      ROS_INFO("Move command failed! Message: %s", Ack->message.c_str());
+      FF_INFO("Move command failed! Message: %s", Ack->message.c_str());
     }
   }
 
   // Command finshed so exit
-  ros::shutdown();
+  rclcpp::shutdown();
 }
 
 int main(int argc, char** argv) {
-  ros::init(argc, argv, "teleop_pub");
-  ros::NodeHandle nh;
+  rclcpp::init(argc, argv);
+  NodeHandle nh;
 
   if (argc <= 1) {
-    ROS_ERROR("Error! Must provide x, y, and z as arguments!");
+    FF_ERROR("Error! Must provide x, y, and z as arguments!");
     return -1;
   }
 
   // Make publisher to publish the command
-  ros::Publisher cmd_publisher;
-  cmd_publisher = nh.advertise<ff_msgs::CommandStamped>(TOPIC_COMMAND, 10);
+  Publisher<ff_msgs::msg::CommandStamped> cmd_publisher =
+      FF_CREATE_PUBLISHER(nh, ff_msgs::msg::CommandStamped, TOPIC_COMMAND, 10);
 
   // Make subscriber to receive command acks to see if the command completed
   // successfully
-  ros::Subscriber ack_subscriber;
-  ack_subscriber = nh.subscribe(TOPIC_MANAGEMENT_ACK, 10, &AckCallback);
+  Subscriber<ff_msgs::msg::AckStamped> ack_subscriber =
+      FF_CREATE_SUBSCRIBER(nh,
+                           ff_msgs::msg::AckStamped,
+                           TOPIC_MANAGEMENT_ACK,
+                           10,
+                           std::bind(&AckCallback, std::placeholders::_1));
 
   // Make ros command message to send to the executive
-  ff_msgs::CommandStamped move_cmd;
-
-  move_cmd.header.stamp = ros::Time::now();
+  ff_msgs::msg::CommandStamped move_cmd;
 
   // Command names listed in CommandConstants.h
-  move_cmd.cmd_name = ff_msgs::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF;
+  move_cmd.cmd_name = ff_msgs::msg::CommandConstants::CMD_NAME_SIMPLE_MOVE6DOF;
 
   // Command id needs to be a unique id that you will use make sure the command
   // was executed, usually a combination of username and timestamp
-  unique_cmd_id = "guest_science" + std::to_string(move_cmd.header.stamp.sec);
+  unique_cmd_id = "guest_science" + std::to_string(nh->get_clock()->now());
   move_cmd.cmd_id = unique_cmd_id;
 
   // Source of the command, set to guest_science so that the system knows that
@@ -92,24 +97,24 @@ int main(int argc, char** argv) {
 
   // Set frame to be world, although I don't believe frame is being used so you
   // can really set it to anything
-  move_cmd.args[0].data_type = ff_msgs::CommandArg::DATA_TYPE_STRING;
+  move_cmd.args[0].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_STRING;
   move_cmd.args[0].s = "world";
 
   // Set location where you want Astrobee to go to
-  move_cmd.args[1].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d;
+  move_cmd.args[1].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_VEC3d;
   move_cmd.args[1].vec3d[0] = atof(argv[1]);  // x
   move_cmd.args[1].vec3d[1] = atof(argv[2]);  // y
   move_cmd.args[1].vec3d[2] = atof(argv[3]);  // z (This axis may not currently work
 
   // Tolerance not used! If you want to set the tolerance, you need to use the
   // set operational limits command. I set tolerance to 0
-  move_cmd.args[2].data_type = ff_msgs::CommandArg::DATA_TYPE_VEC3d;
+  move_cmd.args[2].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_VEC3d;
   move_cmd.args[2].vec3d[0] = 0;
   move_cmd.args[2].vec3d[1] = 0;
   move_cmd.args[2].vec3d[2] = 0;
 
   // Target attitude, quaternion, only the first 4 values are used
-  move_cmd.args[3].data_type = ff_msgs::CommandArg::DATA_TYPE_MAT33f;
+  move_cmd.args[3].data_type = ff_msgs::msg::CommandArg::DATA_TYPE_MAT33f;
   move_cmd.args[3].mat33f[0] = 0;
   move_cmd.args[3].mat33f[1] = 0;
   move_cmd.args[3].mat33f[2] = 0;
@@ -121,9 +126,9 @@ int main(int argc, char** argv) {
   move_cmd.args[3].mat33f[8] = 0;
 
   // Send command
-  cmd_publisher.publish(move_cmd);
+  cmd_publisher->publish(move_cmd);
 
-  ROS_INFO("waiting for executive to execute the command...");
-  ros::spin();
+  FF_INFO("waiting for executive to execute the command...");
+  rclcpp::spin();
   return 0;
 }