made some changes

.vscode/settings.json

@@ -68,6 +68,7 @@
         "typeinfo": "cpp",
         "valarray": "cpp",
         "variant": "cpp",
-        "bit": "cpp"
+        "bit": "cpp",
+        "filesystem": "cpp"
     }
 }

src/lab_1/src/square.cpp

@@ -188,8 +188,6 @@ int main(int argc, char **argv)
 
         std::string out_path = "/home/user/workspace/src/square_eef_points.csv";
 
-        ArmController::extract_eef_from_trajectory(arm_move_group,out_path,trajectory);
-
         n.setParam("/record_pose", true);
 
         arm_move_group.execute(trajectory);

src/lab_2/CMakeLists.txt

@@ -52,4 +52,7 @@ add_executable(scene src/scene.cpp)
 target_link_libraries(scene ${catkin_LIBRARIES})
 
 add_executable(pick_place src/pick_place.cpp)
-target_link_libraries(pick_place ${catkin_LIBRARIES})
+target_link_libraries(pick_place ${catkin_LIBRARIES})
+
+add_executable(pick_place_scene src/pick_place_scene.cpp)
+target_link_libraries(pick_place_scene ${catkin_LIBRARIES})

src/lab_2/src/pick_place_scene.cpp

@@ -0,0 +1,54 @@
+#include "../include/pick_place.hpp"
+
+int main(int argc, char **argv) {
+
+    ros::init(argc, argv, "ppscene");
+    ros::AsyncSpinner spinner(4);
+    spinner.start();
+
+    ros::NodeHandle n;
+
+    moveit::planning_interface::MoveGroupInterface arm_move_group("manipulator");
+
+    std::string planning_frame = arm_move_group.getPlanningFrame();
+
+    // Box
+    moveit_msgs::CollisionObject robot_box_obj;
+    std::string box_id = "box";
+    shape_msgs::SolidPrimitive box_primitive;
+    std::vector<double> box_dim = {0.04, 0.04, 0.04};
+    std::string box_type = "BOX";
+    geometry_msgs::Pose box_pose;
+    box_pose.orientation.w = 1.0;
+    box_pose.position.x = 0.0;
+    box_pose.position.y = 0.40;
+    box_pose.position.z = 0.786;
+    ArmController::createCollisionObject(robot_box_obj, box_id, box_primitive, box_pose, box_type, box_dim, planning_frame);
+
+    // Table
+    moveit_msgs::CollisionObject robot_table_obj;
+    std::string table_id = "table";
+    shape_msgs::SolidPrimitive table_primitive;
+    std::vector<double> table_dim = {1.12, 0.805, 0.766};
+    std::string table_type = "BOX";
+    geometry_msgs::Pose table_pose;
+    table_pose.orientation.w = 1.0;
+    table_pose.position.x = 0.0;
+    table_pose.position.y = 0.3125;
+    table_pose.position.z = 0.383;
+    ArmController::createCollisionObject(robot_table_obj, table_id, table_primitive, table_pose, table_type, table_dim, planning_frame);
+
+    ros::Publisher planning_scene_diff_publisher = n.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
+    moveit_msgs::PlanningScene planning_scene_msg;
+    ArmController::addCollisionObjectToScene(robot_box_obj, planning_scene_msg);
+    ArmController::addCollisionObjectToScene(robot_table_obj, planning_scene_msg);
+
+    planning_scene_msg.is_diff = true;
+    ros::Rate loop_rate(100);
+    while (ros::ok())
+    {
+        planning_scene_diff_publisher.publish(planning_scene_msg);
+        ROS_INFO("published");
+        loop_rate.sleep();
+    }
+}

src/moveit_tutorial/src/tutorial.cpp

@@ -81,8 +81,6 @@ int main(int argc, char **argv){
 
     std::string out_path = "/home/user/workspace/src/tutorial_eef_points.csv";
 
-    ArmController::extract_eef_from_trajectory(arm_move_group,out_path,trajectory);
-
     arm_move_group.execute(trajectory);
 
     ArmController::close_gripper(&n);

src/moveit_wrapper/src/arm_controller.cpp

@@ -187,69 +187,6 @@ moveit_msgs::RobotTrajectory ArmController::planCartesianPath(geometry_msgs::Pos
   return trajectory;
 }
 
-// void ArmController::get_eef_positions(moveit::planning_interface::MoveGroupInterface &move_group_interface, std::string in_path, std::string out_path){
-//   moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
-//   move_group_interface.setStartState(start_state);
-
-//   std::vector<double> joint_val;
-//   std::fstream joint_file;
-//   joint_file.open(in_path,std::ios::in);
-//   std::ofstream eef_file;
-//   eef_file.open(out_path);
-//   if(joint_file.is_open()){
-//     std::string tp;
-//     while(getline(joint_file,tp)){
-//       std::stringstream ss(tp);
-//       while(ss){
-//         double num;
-//         ss >> num;
-//         joint_val.push_back(num);
-//       }
-//       start_state.setJointGroupPositions("manipulator", joint_val);
-//       const Eigen::Affine3d& link_pose = start_state.getGlobalLinkTransform("tool0");
-//       Eigen::Vector3d cartesian_position = link_pose.translation();
-//       for(int i = 0; i < cartesian_position.size(); i++){
-//         if(i == cartesian_position.size()-1){
-//           eef_file << cartesian_position[i] << std::endl;
-//         }
-//         else{
-//           eef_file << cartesian_position[i] << ",";
-//         }
-//       }
-//       joint_val.clear();
-//     }
-//   }
-//   joint_file.close();
-//   eef_file.close();
-// }
-
-void ArmController::extract_eef_from_trajectory(moveit::planning_interface::MoveGroupInterface &move_group_interface, std::string out_path,
-  moveit_msgs::RobotTrajectory trajectory){
-    moveit::core::RobotState robot_state(*move_group_interface.getCurrentState());
-    std::ofstream eef_file;
-    eef_file.open(out_path);
-
-    std::vector<double> joint_values;
-    std::vector<std::vector<double>> eef_values;
-
-    for(int i = 0; i < trajectory.joint_trajectory.points.size(); i++){
-        for(int j = 0; j < trajectory.joint_trajectory.points[i].positions.size(); j++){
-            joint_values.push_back(trajectory.joint_trajectory.points[i].positions[j]);
-        }
-        robot_state.setJointGroupPositions("manipulator", joint_values);
-        const Eigen::Affine3d& link_pose = robot_state.getGlobalLinkTransform("tool0");
-        Eigen::Vector3d cartesian_position = link_pose.translation();
-        for(int k = 0; k < cartesian_position.size(); k++){
-            if(k == cartesian_position.size()-1) eef_file << cartesian_position[k] << std::endl;
-            else eef_file << cartesian_position[k] << ",";
-        }
-
-        joint_values.clear();
-    }
-
-    eef_file.close();
-}
-
 void ArmController::close_gripper(ros::NodeHandle *nh){
   ros::Publisher left_gripper_pub = nh->advertise<control_msgs::GripperCommandActionGoal>("/left_gripper_controller/gripper_cmd/goal", 1);
   ros::Publisher right_gripper_pub = nh->advertise<control_msgs::GripperCommandActionGoal>("/right_gripper_controller/gripper_cmd/goal", 1);