46 #include <rclcpp/rclcpp.hpp>
47 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
48 #include <tf2_ros/transform_listener.h>
55 constexpr
auto K_BASE_FRAME =
"panda_link0";
56 constexpr
auto K_TIP_FRAME =
"panda_link8";
59 int main(
int argc,
char* argv[])
61 rclcpp::init(argc, argv);
64 const rclcpp::Node::SharedPtr demo_node = std::make_shared<rclcpp::Node>(
"moveit_servo_demo");
68 const std::string param_namespace =
"moveit_servo";
69 const std::shared_ptr<const servo::ParamListener> servo_param_listener =
70 std::make_shared<const servo::ParamListener>(demo_node, param_namespace);
71 const servo::Params servo_params = servo_param_listener->get_params();
74 rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr trajectory_outgoing_cmd_pub =
75 demo_node->create_publisher<trajectory_msgs::msg::JointTrajectory>(servo_params.command_out_topic,
76 rclcpp::SystemDefaultsQoS());
84 const auto get_current_pose = [](
const std::string& target_frame,
const moveit::core::RobotStatePtr& robot_state) {
85 return robot_state->getGlobalLinkTransform(target_frame);
90 std::this_thread::sleep_for(std::chrono::seconds(3));
95 robot_state->getJointModelGroup(servo_params.move_group_name);
102 target_pose.
frame_id = K_BASE_FRAME;
105 target_pose.
pose = get_current_pose(K_TIP_FRAME, robot_state);
108 Eigen::Isometry3d terminal_pose = target_pose.
pose;
109 terminal_pose.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ()));
114 Eigen::AngleAxisd angular_step_size(0.01, Eigen::Vector3d::UnitZ());
118 rclcpp::WallRate servo_rate(1 / servo_params.publish_period);
121 std::deque<KinematicState> joint_cmd_rolling_window;
123 updateSlidingWindow(current_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
125 bool satisfies_linear_tolerance =
false;
126 bool satisfies_angular_tolerance =
false;
127 bool stop_tracking =
false;
128 while (!stop_tracking && rclcpp::ok())
131 target_pose.
pose = get_current_pose(K_TIP_FRAME, robot_state);
132 satisfies_linear_tolerance |= target_pose.
pose.translation().isApprox(terminal_pose.translation(),
133 servo_params.pose_tracking.linear_tolerance);
134 satisfies_angular_tolerance |=
135 target_pose.
pose.rotation().isApprox(terminal_pose.rotation(), servo_params.pose_tracking.angular_tolerance);
136 stop_tracking = satisfies_linear_tolerance && satisfies_angular_tolerance;
139 if (!satisfies_linear_tolerance)
141 target_pose.
pose.translate(linear_step_size);
143 if (!satisfies_angular_tolerance)
145 target_pose.
pose.rotate(angular_step_size);
153 updateSlidingWindow(joint_state, joint_cmd_rolling_window, servo_params.max_expected_latency, demo_node->now());
156 trajectory_outgoing_cmd_pub->publish(msg.value());
158 if (!joint_cmd_rolling_window.empty())
160 robot_state->setJointGroupPositions(joint_model_group, joint_cmd_rolling_window.back().positions);
161 robot_state->setJointGroupVelocities(joint_model_group, joint_cmd_rolling_window.back().velocities);
168 RCLCPP_INFO_STREAM(demo_node->get_logger(),
"REACHED : " << stop_tracking);
169 RCLCPP_INFO(demo_node->get_logger(),
"Exiting demo.");
void setCommandType(const CommandType &command_type)
Set the type of incoming servo command.
std::string getStatusMessage() const
Get the message associated with the current servo status.
KinematicState getCurrentRobotState() const
Get the current state of the robot as given by planning scene monitor.
StatusCode getStatus() const
Get the current status of servo.
KinematicState getNextJointState(const moveit::core::RobotStatePtr &robot_state, const ServoInput &command)
Computes the joint state required to follow the given command.
int main(int argc, char *argv[])
Vec3fX< details::Vec3Data< double > > Vector3d
std::optional< trajectory_msgs::msg::JointTrajectory > composeTrajectoryMessage(const servo::Params &servo_params, const std::deque< KinematicState > &joint_cmd_rolling_window)
Create a trajectory message from a rolling window queue of joint state commands. Method optionally re...
void updateSlidingWindow(KinematicState &next_joint_state, std::deque< KinematicState > &joint_cmd_rolling_window, double max_expected_latency, const rclcpp::Time &cur_time)
Adds a new joint state command to a queue containing commands over a time window. Also modifies the v...
planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const rclcpp::Node::SharedPtr &node, const servo::Params &servo_params)
Creates the planning scene monitor used by servo.
void setNodeLoggerName(const std::string &name)
Call once after creating a node to initialize logging namespaces.