From 5ac5ec12826b94e6a9e3bf0c737d7efaa1bfd71b Mon Sep 17 00:00:00 2001
From: mrceki <105711013+mrceki@users.noreply.github.com>
Date: Thu, 2 Mar 2023 08:07:47 +0300
Subject: [PATCH] hotfix

---
 .../launch/demo_ros_control.launch            |  4 +--
 .../launch/planning_context.launch            |  2 +-
 air_moveit_config/src/pick_demo.cpp           | 25 ++++++++++++-------
 .../realsense2_camera/launch/rs_camera.launch | 11 +++++---
 4 files changed, 26 insertions(+), 16 deletions(-)

diff --git a/air_moveit_config/launch/demo_ros_control.launch b/air_moveit_config/launch/demo_ros_control.launch
index c33efe4..c40c184 100644
--- a/air_moveit_config/launch/demo_ros_control.launch
+++ b/air_moveit_config/launch/demo_ros_control.launch
@@ -5,7 +5,7 @@
   <arg name="controller" default="fake" />
   <arg name="moveit_controller_manager" value="$(arg controller)"/>
 
-  <param name="robot_description" textfile="$(find air_description)/urdf/air.urdf" />
+  <!--param name="robot_description" textfile="$(find air_description)/urdf/air.urdf" /-->
 
 <!-- Given the published joint states, publish tf for the robot links -->
   <group if="$(eval arg('moveit_controller_manager') == 'ros_control')">
@@ -17,7 +17,7 @@
 
   <include file="$(dirname)/demo.launch" pass_all_args="true">
     <!-- robot description is loaded by gazebo.launch, to enable Gazebo features -->
-    <arg name="load_robot_description" value="false" />
+    <arg name="load_robot_description" value="true" />
     <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
   </include>
 </launch>
diff --git a/air_moveit_config/launch/planning_context.launch b/air_moveit_config/launch/planning_context.launch
index 7ff2e90..7a43dab 100644
--- a/air_moveit_config/launch/planning_context.launch
+++ b/air_moveit_config/launch/planning_context.launch
@@ -13,7 +13,7 @@
 
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
-    <!--param name="shape_transform_cache_lookup_wait_time" value="0.1" /-->
+    <param name="shape_transform_cache_lookup_wait_time" value="0.2" />
     <rosparam command="load" file="$(find air_moveit_config)/config/joint_limits.yaml"/>
     <rosparam command="load" file="$(find air_moveit_config)/config/cartesian_limits.yaml"/>
   </group>
diff --git a/air_moveit_config/src/pick_demo.cpp b/air_moveit_config/src/pick_demo.cpp
index dd8b82a..a8ea495 100644
--- a/air_moveit_config/src/pick_demo.cpp
+++ b/air_moveit_config/src/pick_demo.cpp
@@ -17,27 +17,32 @@ void air_pick::pick(){
 
   move_group->setPlanningTime(45.0);
   geometry_msgs::Pose pick_pose, place_pose;
-  geometry_msgs::PoseStamped current_pose;
+  //geometry_msgs::PoseStamped current_pose;
 
   ROS_INFO("Object Pose: x= %f, y= %f, z= %f ",m_x,m_y,m_z);
 
   // Pick Pose
-  current_pose = move_group->getCurrentPose("tool0");
-  pick_pose.position.x = m_x;
-  pick_pose.position.y = m_y;
-  pick_pose.position.z = m_z + 0.05;
-  pick_pose.orientation = current_pose.pose.orientation;
-
+  //current_pose = move_group->getCurrentPose("tool0");
+  pick_pose.position.x = 0.63;//m_x;
+  pick_pose.position.y = 0.2;//m_y;
+  pick_pose.position.z = 0.85;//m_z + 0.05;
+  pick_pose.orientation.w = 0.5;
+  pick_pose.orientation.x = -0.5;
+  pick_pose.orientation.y = 0.5;
+  pick_pose.orientation.z = -0.5;
+  ROS_INFO("POSE GENERATED");
   // Place Pose
   place_pose.position.x = m_x;
   place_pose.position.y = m_y + 0,05;
   place_pose.position.z = m_z + 0.1;
   move_group->setPoseTarget(pick_pose);
+  ROS_INFO("POSE TARGET SETTED");
 
   // Plan and execute the pick motion
   moveit::planning_interface::MoveGroupInterface::Plan plan;
   moveit::planning_interface::MoveItErrorCode result = move_group->plan(plan);
-  
+    ROS_INFO("PLANNED");
+
   if (result == moveit::planning_interface::MoveItErrorCode::SUCCESS)
   {
     move_group->execute(plan);
@@ -77,9 +82,11 @@ void air_pick::planningSceneWorldCallback(const moveit_msgs::PlanningScene::Cons
 
 int main(int argc, char** argv)
 {
+
   ros::init(argc, argv, "pick_and_place");
+
   ros::NodeHandle nh;
   air_pick obj(nh);
-  ros::spinOnce();
+  ros::spin();
   return 0;
 }
\ No newline at end of file
diff --git a/realsense-ros/realsense2_camera/launch/rs_camera.launch b/realsense-ros/realsense2_camera/launch/rs_camera.launch
index 48a3fbb..d235e12 100644
--- a/realsense-ros/realsense2_camera/launch/rs_camera.launch
+++ b/realsense-ros/realsense2_camera/launch/rs_camera.launch
@@ -1,4 +1,5 @@
 <launch>
+
   <arg name="serial_no"           default=""/>
   <arg name="usb_port_id"         default=""/>
   <arg name="device_type"         default=""/>
@@ -8,7 +9,7 @@
   <arg name="external_manager"    default="false"/>
   <arg name="manager"             default="realsense2_camera_manager"/>
   <arg name="output"              default="screen"/>
-  <arg name="respawn"              default="false"/>
+  <arg name="respawn"             default="false"/>
 
   <arg name="fisheye_width"       default="640"/>
   <arg name="fisheye_height"      default="480"/>
@@ -55,7 +56,7 @@
   <arg name="publish_tf"                default="true"/>
   <arg name="tf_publish_rate"           default="0"/>
 
-  <arg name="filters"                   default=""/>
+  <arg name="filters"                   default="pointcloud"/>
   <arg name="clip_distance"             default="-2"/>
   <arg name="linear_accel_cov"          default="0.01"/>
   <arg name="initial_reset"             default="true"/>
@@ -66,11 +67,13 @@
   <arg name="calib_odom_file"           default=""/>
   <arg name="publish_odom_tf"           default="false"/>
 
-  <arg name="stereo_module/exposure/1"  default="7500"/>true
+  <arg name="stereo_module/exposure/1"  default="7500"/>
   <arg name="stereo_module/gain/1"      default="16"/>
   <arg name="stereo_module/exposure/2"  default="1"/>
   <arg name="stereo_module/gain/2"      default="16"/>
-  
+  <param name="/camera/motion_module/global_time_enabled" type="bool" value="true"/>
+  <param name="/camera/l500_depth_sensor/global_time_enabled" type="bool" value="true"/>
+  <param name="/camera/rgb_camera/global_time_enabled" type="bool" value="true"/>
   
 
   <group ns="$(arg camera)">