サンプル実行時の初期姿勢を追加 (#159)

rt-net · Sep 6, 2022 · 2aa7d37 · 2aa7d37
1 parent dfab983
commit 2aa7d37
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 10 deletions.
diff --git a/crane_x7_examples/src/gripper_control.cpp b/crane_x7_examples/src/gripper_control.cpp
@@ -31,18 +31,27 @@ int main(int argc, char ** argv)
   rclcpp::init(argc, argv);
   rclcpp::NodeOptions node_options;
   node_options.automatically_declare_parameters_from_overrides(true);
+  auto move_group_arm_node = rclcpp::Node::make_shared("move_group_arm_node", node_options);
   auto move_group_gripper_node = rclcpp::Node::make_shared("move_group_gripper_node", node_options);
   // For current state monitor
   rclcpp::executors::SingleThreadedExecutor executor;
+  executor.add_node(move_group_arm_node);
   executor.add_node(move_group_gripper_node);
   std::thread([&executor]() {executor.spin();}).detach();
 
+  MoveGroupInterface move_group_arm(move_group_arm_node, "arm");
+  move_group_arm.setMaxVelocityScalingFactor(1.0);  // Set 0.0 ~ 1.0
+  move_group_arm.setMaxAccelerationScalingFactor(1.0);  // Set 0.0 ~ 1.0
+
   MoveGroupInterface move_group_gripper(move_group_gripper_node, "gripper");
   move_group_gripper.setMaxVelocityScalingFactor(1.0);  // Set 0.0 ~ 1.0
   move_group_gripper.setMaxAccelerationScalingFactor(1.0);  // Set 0.0 ~ 1.0
-
   auto gripper_joint_values = move_group_gripper.getCurrentJointValues();
 
+  // SRDFに定義されている"home"の姿勢にする
+  move_group_arm.setNamedTarget("home");
+  move_group_arm.move();
+
   gripper_joint_values[0] = angles::from_degrees(60);
   move_group_gripper.setJointValueTarget(gripper_joint_values);
   move_group_gripper.move();

diff --git a/crane_x7_examples/src/pick_and_place.cpp b/crane_x7_examples/src/pick_and_place.cpp
@@ -55,15 +55,15 @@ int main(int argc, char ** argv)
   double GRIPPER_OPEN = angles::from_degrees(60.0);
   double GRIPPER_CLOSE = angles::from_degrees(20);
 
+  // SRDFに定義されている"home"の姿勢にする
+  move_group_arm.setNamedTarget("home");
+  move_group_arm.move();
+
   // 何かを掴んでいた時のためにハンドを開く
   gripper_joint_values[0] = GRIPPER_OPEN;
   move_group_gripper.setJointValueTarget(gripper_joint_values);
   move_group_gripper.move();
 
-  // SRDFに定義されている"home"の姿勢にする
-  move_group_arm.setNamedTarget("home");
-  move_group_arm.move();
-
   // 可動範囲を制限する
   moveit_msgs::msg::Constraints constraints;
   constraints.name = "arm_constraints";
@@ -96,11 +96,6 @@ int main(int argc, char ** argv)
   move_group_arm.setPoseTarget(target_pose);
   move_group_arm.move();
 
-  // ハンドを開く
-  gripper_joint_values[0] = GRIPPER_OPEN;
-  move_group_gripper.setJointValueTarget(gripper_joint_values);
-  move_group_gripper.move();
-
   // 掴みに行く
   target_pose.position.x = 0.2;
   target_pose.position.y = 0.0;