瀏覽代碼

2024-9-27-2

bzx 8 月之前
父節點
當前提交
4087cc5a4a
共有 46 個文件被更改,包括 82 次插入85 次删除
  1. 1 1
      ren_yuan/src/lst_robot_r/CMakeLists.txt
  2. 1 1
      ren_yuan/src/lst_robot_r/scripts/command.py
  3. 7 7
      ren_yuan/src/lst_robot_r/scripts/start.py
  4. 0 10
      ren_yuan/src/lstrobot_moveit_config_0605/config/sensors_3d2.yaml
  5. 1 1
      ren_yuan/src/robot_config/.setup_assistant
  6. 1 1
      ren_yuan/src/robot_config/CMakeLists.txt
  7. 1 1
      ren_yuan/src/robot_config/config/a9.srdf
  8. 0 0
      ren_yuan/src/robot_config/config/cartesian_limits.yaml
  9. 0 0
      ren_yuan/src/robot_config/config/chomp_planning.yaml
  10. 0 0
      ren_yuan/src/robot_config/config/fake_controllers.yaml
  11. 18 6
      ren_yuan/src/robot_config/config/gazebo_a9.urdf
  12. 0 0
      ren_yuan/src/robot_config/config/gazebo_controllers.yaml
  13. 0 0
      ren_yuan/src/robot_config/config/joint_limits.yaml
  14. 0 0
      ren_yuan/src/robot_config/config/kinematics.yaml
  15. 0 2
      ren_yuan/src/robot_config/config/ompl_planning.yaml
  16. 0 0
      ren_yuan/src/robot_config/config/ros_controllers.yaml
  17. 1 2
      ren_yuan/src/robot_config/config/sensors_3d.yaml
  18. 0 0
      ren_yuan/src/robot_config/config/simple_moveit_controllers.yaml
  19. 0 0
      ren_yuan/src/robot_config/config/stomp_planning.yaml
  20. 0 0
      ren_yuan/src/robot_config/launch/a9_moveit_sensor_manager.launch.xml
  21. 1 1
      ren_yuan/src/robot_config/launch/chomp_planning_pipeline.launch.xml
  22. 1 1
      ren_yuan/src/robot_config/launch/default_warehouse_db.launch
  23. 1 1
      ren_yuan/src/robot_config/launch/demo.launch
  24. 0 0
      ren_yuan/src/robot_config/launch/demo_gazebo.launch
  25. 1 1
      ren_yuan/src/robot_config/launch/fake_moveit_controller_manager.launch.xml
  26. 2 2
      ren_yuan/src/robot_config/launch/gazebo.launch
  27. 0 0
      ren_yuan/src/robot_config/launch/joystick_control.launch
  28. 0 0
      ren_yuan/src/robot_config/launch/move_group.launch
  29. 30 32
      ren_yuan/src/robot_config/launch/moveit.rviz
  30. 0 0
      ren_yuan/src/robot_config/launch/moveit_rviz.launch
  31. 2 2
      ren_yuan/src/robot_config/launch/ompl-chomp_planning_pipeline.launch.xml
  32. 1 1
      ren_yuan/src/robot_config/launch/ompl_planning_pipeline.launch.xml
  33. 0 0
      ren_yuan/src/robot_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
  34. 4 4
      ren_yuan/src/robot_config/launch/planning_context.launch
  35. 0 0
      ren_yuan/src/robot_config/launch/planning_pipeline.launch.xml
  36. 0 0
      ren_yuan/src/robot_config/launch/ros_control_moveit_controller_manager.launch.xml
  37. 1 1
      ren_yuan/src/robot_config/launch/ros_controllers.launch
  38. 1 1
      ren_yuan/src/robot_config/launch/run_benchmark_ompl.launch
  39. 1 1
      ren_yuan/src/robot_config/launch/sensor_manager.launch.xml
  40. 1 1
      ren_yuan/src/robot_config/launch/setup_assistant.launch
  41. 2 2
      ren_yuan/src/robot_config/launch/simple_moveit_controller_manager.launch.xml
  42. 1 1
      ren_yuan/src/robot_config/launch/stomp_planning_pipeline.launch.xml
  43. 0 0
      ren_yuan/src/robot_config/launch/trajectory_execution.launch.xml
  44. 0 0
      ren_yuan/src/robot_config/launch/warehouse.launch
  45. 0 0
      ren_yuan/src/robot_config/launch/warehouse_settings.launch.xml
  46. 1 1
      ren_yuan/src/robot_config/package.xml

+ 1 - 1
ren_yuan/src/lst_robot_r/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
 cmake_minimum_required(VERSION 3.0.2)
-project(lstrobot_planning)
+project(lst_robot_r)
 
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
 # add_compile_options(-std=c++11)

+ 1 - 1
ren_yuan/src/lst_robot_r/scripts/command.py

@@ -25,7 +25,7 @@ def load_visual():
     command = "rosrun visual visual_node"
     command = "rosrun visual visual_node"
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
 def load_rviz():
 def load_rviz():
-    command = "roslaunch lstrobot_moveit_config_0605 demo.launch"
+    command = "roslaunch robot_config demo.launch"
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
     subprocess.Popen(['gnome-terminal', '--', 'bash', '-c', command])
 def launch_publish_pointcloud():
 def launch_publish_pointcloud():
     file_name = "dycl_0506.py"
     file_name = "dycl_0506.py"

+ 7 - 7
ren_yuan/src/lst_robot_r/scripts/start.py

@@ -47,7 +47,7 @@ if __name__ == "__main__":
     rospy.set_param("sign_control",1)
     rospy.set_param("sign_control",1)
     rospy.set_param("sign_pointcloud",0)
     rospy.set_param("sign_pointcloud",0)
 
 
-    rospy.set_param("folder_path","/home/robot/ROS/ren_yuan/data/2")
+    rospy.set_param("folder_path","/home/robot/ROS/ren_yuan/data/3")
 
 
    
    
     rospy.loginfo("等待rviz启动")
     rospy.loginfo("等待rviz启动")
@@ -68,7 +68,7 @@ if __name__ == "__main__":
                 if(aaa =='1' or aaa=='2'):
                 if(aaa =='1' or aaa=='2'):
                     rospy.set_param("sign_control",aaa)
                     rospy.set_param("sign_control",aaa)
                 else:
                 else:
-                    command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch lstrobot_moveit_config_0605 demo.launch")
+                    command.close_rviz("/usr/bin/python3 /opt/ros/noetic/bin/roslaunch robot_config demo.launch")
                     exit(0)
                     exit(0)
                 waited_once = True
                 waited_once = True
         elif sign_control == "1":
         elif sign_control == "1":
@@ -78,17 +78,17 @@ if __name__ == "__main__":
             # 清除场景
             # 清除场景
             clear_octomap()
             clear_octomap()
             #点云计算并发布
             #点云计算并发布
-            process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
+            # process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
-            process.start()
+            # process.start()
             #计算焊接顺序和焊接姿态
             #计算焊接顺序和焊接姿态
             hjsx.run()
             hjsx.run()
             hanqiangpose.run()
             hanqiangpose.run()
             time.sleep(10)
             time.sleep(10)
             command.load_visual()
             command.load_visual()
             # 等待 /move_group/monitored_planning_scene 话题发布消息
             # 等待 /move_group/monitored_planning_scene 话题发布消息
-            rospy.loginfo("等待场景地图加载完毕...")
+            # rospy.loginfo("等待场景地图加载完毕...")
-            wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+            # wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
-            rospy.loginfo(f"场景地图已加载完毕")
+            # rospy.loginfo(f"场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
             rospy.set_param("sign_pointcloud",1)
             #rospy.loginfo("终止点云发布,关闭发布窗口")
             #rospy.loginfo("终止点云发布,关闭发布窗口")
             
             

+ 0 - 10
ren_yuan/src/lstrobot_moveit_config_0605/config/sensors_3d2.yaml

@@ -1,10 +0,0 @@
-sensors2:
-  - filtered_cloud_topic: filtered_cloud
-    max_range: 5.0
-    max_update_rate: 1.0
-    padding_offset: 0.1
-    padding_scale: 1.0
-    point_cloud_topic: /pointcloud/output2
-    point_subsample: 1
-    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/.setup_assistant → ren_yuan/src/robot_config/.setup_assistant

@@ -8,4 +8,4 @@ moveit_setup_assistant_config:
   CONFIG:
   CONFIG:
     author_name: 123
     author_name: 123
     author_email: 123@163.com
     author_email: 123@163.com
-    generated_timestamp: 1727315613
+    generated_timestamp: 1727407047

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/CMakeLists.txt → ren_yuan/src/robot_config/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.1.3)
 cmake_minimum_required(VERSION 3.1.3)
-project(lstrobot_moveit_config_0605)
+project(robot_config)
 
 
 find_package(catkin REQUIRED)
 find_package(catkin REQUIRED)
 
 

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/config/a9.srdf → ren_yuan/src/robot_config/config/a9.srdf

@@ -10,7 +10,7 @@
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
     <group name="manipulator">
     <group name="manipulator">
-        <chain base_link="base_link" tip_link="end_link_now"/>
+        <chain base_link="world" tip_link="end_link_now"/>
     </group>
     </group>
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
     <group_state name="zero" group="manipulator">
     <group_state name="zero" group="manipulator">

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/cartesian_limits.yaml → ren_yuan/src/robot_config/config/cartesian_limits.yaml


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/chomp_planning.yaml → ren_yuan/src/robot_config/config/chomp_planning.yaml


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/fake_controllers.yaml → ren_yuan/src/robot_config/config/fake_controllers.yaml


+ 18 - 6
ren_yuan/src/lstrobot_moveit_config_0605/config/gazebo_a9.urdf → ren_yuan/src/robot_config/config/gazebo_a9.urdf

@@ -3,6 +3,12 @@
      Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
      Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
      For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
      For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
 <robot name="a9">
 <robot name="a9">
+    <link name="world" />
+    <joint name="world_joint" type="fixed">
+        <parent link="world" />
+        <child link="base_link" />
+        <origin xyz="0.0 0.0 2.17" rpy="0.0 3.14 0.0" />
+    </joint>
     <link name="base_link">
     <link name="base_link">
         <inertial>
         <inertial>
             <origin xyz="-0.110121070288428 -0.0129999995820303 0.159000000973282" rpy="0 0 0" />
             <origin xyz="-0.110121070288428 -0.0129999995820303 0.159000000973282" rpy="0 0 0" />
@@ -52,7 +58,7 @@
         <parent link="base_link" />
         <parent link="base_link" />
         <child link="Link_1" />
         <child link="Link_1" />
         <axis xyz="0 0 1" />
         <axis xyz="0 0 1" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+        <limit lower="-3.8" upper="3.8" effort="0" velocity="0" />
     </joint>
     </joint>
     <link name="Link_2">
     <link name="Link_2">
         <inertial>
         <inertial>
@@ -81,7 +87,7 @@
         <parent link="Link_1" />
         <parent link="Link_1" />
         <child link="Link_2" />
         <child link="Link_2" />
         <axis xyz="0 1 0" />
         <axis xyz="0 1 0" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+        <limit lower="-3.8" upper="3.8" effort="0" velocity="0" />
     </joint>
     </joint>
     <link name="Link_3">
     <link name="Link_3">
         <inertial>
         <inertial>
@@ -110,7 +116,7 @@
         <parent link="Link_2" />
         <parent link="Link_2" />
         <child link="Link_3" />
         <child link="Link_3" />
         <axis xyz="0 1 0" />
         <axis xyz="0 1 0" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+        <limit lower="-3.8" upper="3.8" effort="0" velocity="0" />
     </joint>
     </joint>
     <link name="Link_4">
     <link name="Link_4">
         <inertial>
         <inertial>
@@ -139,7 +145,7 @@
         <parent link="Link_3" />
         <parent link="Link_3" />
         <child link="Link_4" />
         <child link="Link_4" />
         <axis xyz="0.86603 0 0.5" />
         <axis xyz="0.86603 0 0.5" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+        <limit lower="-3.8" upper="3.8" effort="0" velocity="0" />
     </joint>
     </joint>
     <link name="Link_5">
     <link name="Link_5">
         <inertial>
         <inertial>
@@ -168,7 +174,7 @@
         <parent link="Link_4" />
         <parent link="Link_4" />
         <child link="Link_5" />
         <child link="Link_5" />
         <axis xyz="0 1 0" />
         <axis xyz="0 1 0" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+        <limit lower="-3.8" upper="3.8" effort="0" velocity="0" />
     </joint>
     </joint>
     <link name="Link_6">
     <link name="Link_6">
         <inertial>
         <inertial>
@@ -197,7 +203,7 @@
         <parent link="Link_5" />
         <parent link="Link_5" />
         <child link="Link_6" />
         <child link="Link_6" />
         <axis xyz="0.49999 0 -0.86603" />
         <axis xyz="0.49999 0 -0.86603" />
-        <limit lower="-3" upper="3" effort="0" velocity="0" />
+        <limit lower="-3.8" upper="3.8" effort="0" velocity="0" />
     </joint>
     </joint>
     <link name="Link_7">
     <link name="Link_7">
         <inertial>
         <inertial>
@@ -227,6 +233,12 @@
         <child link="Link_7" />
         <child link="Link_7" />
         <axis xyz="0 0 0" />
         <axis xyz="0 0 0" />
     </joint>
     </joint>
+    <link name="end_link_now" />
+    <joint name="end_joint" type="fixed">
+        <parent link="Link_7" />
+        <child link="end_link_now" />
+        <origin xyz="-0.045 0.0 0.4" rpy="0.0 0.4 3.14" />
+    </joint>
     <transmission name="trans_joint_1">
     <transmission name="trans_joint_1">
         <type>transmission_interface/SimpleTransmission</type>
         <type>transmission_interface/SimpleTransmission</type>
         <joint name="joint_1">
         <joint name="joint_1">

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/gazebo_controllers.yaml → ren_yuan/src/robot_config/config/gazebo_controllers.yaml


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/joint_limits.yaml → ren_yuan/src/robot_config/config/joint_limits.yaml


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/kinematics.yaml → ren_yuan/src/robot_config/config/kinematics.yaml


+ 0 - 2
ren_yuan/src/lstrobot_moveit_config_0605/config/ompl_planning.yaml → ren_yuan/src/robot_config/config/ompl_planning.yaml

@@ -194,5 +194,3 @@ manipulator:
     - AITstar
     - AITstar
     - ABITstar
     - ABITstar
     - BITstar
     - BITstar
-  projection_evaluator: joints(joint_1,joint_2)
-  longest_valid_segment_fraction: 0.005

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/ros_controllers.yaml → ren_yuan/src/robot_config/config/ros_controllers.yaml


+ 1 - 2
ren_yuan/src/lstrobot_moveit_config_0605/config/sensors_3d.yaml → ren_yuan/src/robot_config/config/sensors_3d.yaml

@@ -6,5 +6,4 @@ sensors:
     padding_scale: 1.0
     padding_scale: 1.0
     point_cloud_topic: /pointcloud/output
     point_cloud_topic: /pointcloud/output
     point_subsample: 1
     point_subsample: 1
-    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/simple_moveit_controllers.yaml → ren_yuan/src/robot_config/config/simple_moveit_controllers.yaml


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/config/stomp_planning.yaml → ren_yuan/src/robot_config/config/stomp_planning.yaml


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/a9_moveit_sensor_manager.launch.xml → ren_yuan/src/robot_config/launch/a9_moveit_sensor_manager.launch.xml


+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/chomp_planning_pipeline.launch.xml → ren_yuan/src/robot_config/launch/chomp_planning_pipeline.launch.xml

@@ -17,5 +17,5 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
 
-  <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/chomp_planning.yaml" />
+  <rosparam command="load" file="$(find robot_config)/config/chomp_planning.yaml" />
 </launch>
 </launch>

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/default_warehouse_db.launch → ren_yuan/src/robot_config/launch/default_warehouse_db.launch

@@ -2,7 +2,7 @@
 
 
   <arg name="reset" default="false"/>
   <arg name="reset" default="false"/>
   <!-- If not specified, we'll use a default database location -->
   <!-- If not specified, we'll use a default database location -->
-  <arg name="moveit_warehouse_database_path" default="$(find lstrobot_moveit_config_0605)/default_warehouse_mongo_db" />
+  <arg name="moveit_warehouse_database_path" default="$(find robot_config)/default_warehouse_mongo_db" />
 
 
   <!-- Launch the warehouse with the configured database location -->
   <!-- Launch the warehouse with the configured database location -->
   <include file="$(dirname)/warehouse.launch">
   <include file="$(dirname)/warehouse.launch">

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/demo.launch → ren_yuan/src/robot_config/launch/demo.launch

@@ -6,7 +6,7 @@
   <!-- By default, we do not start a database (it can be large) -->
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
   <arg name="db" default="false" />
   <!-- Allow user to specify database location -->
   <!-- Allow user to specify database location -->
-  <arg name="db_path" default="$(find lstrobot_moveit_config_0605)/default_warehouse_mongo_db" />
+  <arg name="db_path" default="$(find robot_config)/default_warehouse_mongo_db" />
 
 
   <!-- By default, we are not in debug mode -->
   <!-- By default, we are not in debug mode -->
   <arg name="debug" default="false" />
   <arg name="debug" default="false" />

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/demo_gazebo.launch → ren_yuan/src/robot_config/launch/demo_gazebo.launch


+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/fake_moveit_controller_manager.launch.xml → ren_yuan/src/robot_config/launch/fake_moveit_controller_manager.launch.xml

@@ -7,6 +7,6 @@
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
 
   <!-- The rest of the params are specific to this plugin -->
   <!-- The rest of the params are specific to this plugin -->
-  <rosparam subst_value="true" file="$(find lstrobot_moveit_config_0605)/config/fake_controllers.yaml"/>
+  <rosparam subst_value="true" file="$(find robot_config)/config/fake_controllers.yaml"/>
 
 
 </launch>
 </launch>

+ 2 - 2
ren_yuan/src/lstrobot_moveit_config_0605/launch/gazebo.launch → ren_yuan/src/robot_config/launch/gazebo.launch

@@ -13,7 +13,7 @@
   </include>
   </include>
 
 
   <!-- Set the robot urdf on the parameter server -->
   <!-- Set the robot urdf on the parameter server -->
-  <param name="robot_description" textfile="$(find lstrobot_moveit_config_0605)/config/gazebo_a9.urdf" />
+  <param name="robot_description" textfile="$(find robot_config)/config/gazebo_a9.urdf" />
 
 
   <!-- Unpause the simulation after loading the robot model -->
   <!-- Unpause the simulation after loading the robot model -->
   <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
   <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
@@ -23,7 +23,7 @@
     respawn="false" output="screen" />
     respawn="false" output="screen" />
 
 
   <!-- Load the controller parameters onto the parameter server -->
   <!-- Load the controller parameters onto the parameter server -->
-  <rosparam file="$(find lstrobot_moveit_config_0605)/config/gazebo_controllers.yaml" />
+  <rosparam file="$(find robot_config)/config/gazebo_controllers.yaml" />
   <include file="$(dirname)/ros_controllers.launch"/>
   <include file="$(dirname)/ros_controllers.launch"/>
 
 
   <!-- Spawn the Gazebo ROS controllers -->
   <!-- Spawn the Gazebo ROS controllers -->

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/joystick_control.launch → ren_yuan/src/robot_config/launch/joystick_control.launch


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/move_group.launch → ren_yuan/src/robot_config/launch/move_group.launch


+ 30 - 32
ren_yuan/src/lstrobot_moveit_config_0605/launch/moveit.rviz → ren_yuan/src/robot_config/launch/moveit.rviz

@@ -5,6 +5,8 @@ Panels:
     Property Tree Widget:
     Property Tree Widget:
       Expanded:
       Expanded:
         - /Global Options1
         - /Global Options1
+        - /Grid1
+        - /MotionPlanning1
         - /MotionPlanning1/Scene Geometry1
         - /MotionPlanning1/Scene Geometry1
       Splitter Ratio: 0.5
       Splitter Ratio: 0.5
     Tree Height: 808
     Tree Height: 808
@@ -219,27 +221,6 @@ Visualization Manager:
         Show Robot Visual: false
         Show Robot Visual: false
       Value: true
       Value: true
       Velocity_Scaling_Factor: 0.1
       Velocity_Scaling_Factor: 0.1
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: rviz_visual_tools
-      Name: MarkerArray
-      Namespaces:
-        Path: true
-        Text: true
-        point0  0.983265  0.126875  0.473115: true
-        point0  0.983765  -0.262875  0.473115: true
-        point1  0.983378  0.122593  0.278064: true
-        point1  0.983994  -0.262691  0.279523: true
-        point2  0.983265  0.126875  0.278115: true
-        point2  1.125382  0.127069  0.280059: true
-        point3  0.983994  -0.261691  0.278523: true
-        point3  1.125382  -0.262691  0.280059: true
-        point4  0.983294  -0.262191  0.463923: true
-        point4  0.984994  -0.263491  0.277923: true
-        point5  0.980265  0.126875  0.278115: true
-        point5  0.983365  0.126875  0.468115: true
-      Queue Size: 100
-      Value: true
     - Alpha: 1
     - Alpha: 1
       Class: rviz/RobotModel
       Class: rviz/RobotModel
       Collision Enabled: false
       Collision Enabled: false
@@ -304,10 +285,27 @@ Visualization Manager:
       Update Interval: 0
       Update Interval: 0
       Value: true
       Value: true
       Visual Enabled: true
       Visual Enabled: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        Path: true
+        Text: true
+        point0  0.983265  0.126875  1.478115: true
+        point0  1.125382  0.127069  1.480059: true
+        point1  0.983994  -0.261691  1.478523: true
+        point1  1.125382  -0.262691  1.480059: true
+        point2  0.983294  -0.262191  1.663923: true
+        point2  0.984994  -0.263491  1.477923: true
+        point3  0.980265  0.126875  1.478115: true
+        point3  0.983365  0.126875  1.668115: true
+      Queue Size: 100
+      Value: true
     - Alpha: 1
     - Alpha: 1
       Class: rviz/Axes
       Class: rviz/Axes
       Enabled: true
       Enabled: true
-      Length: 0.05000000074505806
+      Length: 0.054999999701976776
       Name: Axes
       Name: Axes
       Radius: 0.009999999776482582
       Radius: 0.009999999776482582
       Reference Frame: end_link_now
       Reference Frame: end_link_now
@@ -319,7 +317,7 @@ Visualization Manager:
       Filter (whitelist): ""
       Filter (whitelist): ""
       Frame Timeout: 15
       Frame Timeout: 15
       Frames:
       Frames:
-        All Enabled: false
+        All Enabled: true
       Marker Alpha: 1
       Marker Alpha: 1
       Marker Scale: 1
       Marker Scale: 1
       Name: TF
       Name: TF
@@ -334,7 +332,7 @@ Visualization Manager:
   Global Options:
   Global Options:
     Background Color: 48; 48; 48
     Background Color: 48; 48; 48
     Default Light: true
     Default Light: true
-    Fixed Frame: base_link
+    Fixed Frame: world
     Frame Rate: 30
     Frame Rate: 30
   Name: root
   Name: root
   Tools:
   Tools:
@@ -346,7 +344,7 @@ Visualization Manager:
   Views:
   Views:
     Current:
     Current:
       Class: rviz/Orbit
       Class: rviz/Orbit
-      Distance: 3.868440866470337
+      Distance: 2.1756603717803955
       Enable Stereo Rendering:
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Stereo Focal Distance: 1
@@ -354,17 +352,17 @@ Visualization Manager:
         Value: false
         Value: false
       Field of View: 0.75
       Field of View: 0.75
       Focal Point:
       Focal Point:
-        X: 0.43320751190185547
+        X: -0.8280303478240967
-        Y: 0.5569202899932861
+        Y: -0.09661253541707993
-        Z: -0.5814682245254517
+        Z: 0.6303508281707764
       Focal Shape Fixed Size: true
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Invert Z Axis: false
       Name: Current View
       Name: Current View
       Near Clip Distance: 0.009999999776482582
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7650017142295837
+      Pitch: 0.15500007569789886
-      Target Frame: base_link
+      Target Frame: world
-      Yaw: 5.316291332244873
+      Yaw: 1.8181167840957642
     Saved: ~
     Saved: ~
 Window Geometry:
 Window Geometry:
   Displays:
   Displays:
@@ -378,7 +376,7 @@ Window Geometry:
     collapsed: false
     collapsed: false
   MotionPlanning - Trajectory Slider:
   MotionPlanning - Trajectory Slider:
     collapsed: false
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000007fb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000001880000017d00ffffff00000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000007fb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000208000001ee0000017d00ffffff00000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
   Views:
     collapsed: false
     collapsed: false
   Width: 1920
   Width: 1920

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/moveit_rviz.launch → ren_yuan/src/robot_config/launch/moveit_rviz.launch


+ 2 - 2
ren_yuan/src/lstrobot_moveit_config_0605/launch/ompl-chomp_planning_pipeline.launch.xml → ren_yuan/src/robot_config/launch/ompl-chomp_planning_pipeline.launch.xml

@@ -1,6 +1,6 @@
 <launch>
 <launch>
   <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
   <!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
-  <include file="$(find lstrobot_moveit_config_0605)/launch/ompl_planning_pipeline.launch.xml">
+  <include file="$(find robot_config)/launch/ompl_planning_pipeline.launch.xml">
     <arg name="planning_adapters"
     <arg name="planning_adapters"
          default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
          default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
                   default_planner_request_adapters/AddTimeParameterization
                   default_planner_request_adapters/AddTimeParameterization
@@ -13,7 +13,7 @@
   </include>
   </include>
 
 
   <!-- load chomp config -->
   <!-- load chomp config -->
-  <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/chomp_planning.yaml" />
+  <rosparam command="load" file="$(find robot_config)/config/chomp_planning.yaml" />
 
 
   <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
   <!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
   <param name="trajectory_initialization_method" value="fillTrajectory"/>
   <param name="trajectory_initialization_method" value="fillTrajectory"/>

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/ompl_planning_pipeline.launch.xml → ren_yuan/src/robot_config/launch/ompl_planning_pipeline.launch.xml

@@ -19,6 +19,6 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
 
-  <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/ompl_planning.yaml"/>
+  <rosparam command="load" file="$(find robot_config)/config/ompl_planning.yaml"/>
 
 
 </launch>
 </launch>

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml → ren_yuan/src/robot_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml


+ 4 - 4
ren_yuan/src/lstrobot_moveit_config_0605/launch/planning_context.launch → ren_yuan/src/robot_config/launch/planning_context.launch

@@ -9,17 +9,17 @@
   <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find a9)/urdf/a9.urdf"/>
   <param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find a9)/urdf/a9.urdf"/>
 
 
   <!-- The semantic description that corresponds to the URDF -->
   <!-- The semantic description that corresponds to the URDF -->
-  <param name="$(arg robot_description)_semantic" textfile="$(find lstrobot_moveit_config_0605)/config/a9.srdf" />
+  <param name="$(arg robot_description)_semantic" textfile="$(find robot_config)/config/a9.srdf" />
 
 
   <!-- Load updated joint limits (override information from URDF) -->
   <!-- Load updated joint limits (override information from URDF) -->
   <group ns="$(arg robot_description)_planning">
   <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find robot_config)/config/joint_limits.yaml"/>
-    <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/cartesian_limits.yaml"/>
+    <rosparam command="load" file="$(find robot_config)/config/cartesian_limits.yaml"/>
   </group>
   </group>
 
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <group ns="$(arg robot_description)_kinematics">
   <group ns="$(arg robot_description)_kinematics">
-    <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/kinematics.yaml"/>
+    <rosparam command="load" file="$(find robot_config)/config/kinematics.yaml"/>
 
 
   </group>
   </group>
 
 

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/planning_pipeline.launch.xml → ren_yuan/src/robot_config/launch/planning_pipeline.launch.xml


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/ros_control_moveit_controller_manager.launch.xml → ren_yuan/src/robot_config/launch/ros_control_moveit_controller_manager.launch.xml


+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/ros_controllers.launch → ren_yuan/src/robot_config/launch/ros_controllers.launch

@@ -2,7 +2,7 @@
 <launch>
 <launch>
 
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <!-- Load joint controller configurations from YAML file to parameter server -->
-  <rosparam file="$(find lstrobot_moveit_config_0605)/config/ros_controllers.yaml" command="load"/>
+  <rosparam file="$(find robot_config)/config/ros_controllers.yaml" command="load"/>
 
 
   <!-- Load the controllers -->
   <!-- Load the controllers -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/run_benchmark_ompl.launch → ren_yuan/src/robot_config/launch/run_benchmark_ompl.launch

@@ -15,7 +15,7 @@
 
 
   <!-- Start Benchmark Executable -->
   <!-- Start Benchmark Executable -->
   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
   <node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
-    <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/ompl_planning.yaml"/>
+    <rosparam command="load" file="$(find robot_config)/config/ompl_planning.yaml"/>
   </node>
   </node>
 
 
 </launch>
 </launch>

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/sensor_manager.launch.xml → ren_yuan/src/robot_config/launch/sensor_manager.launch.xml

@@ -3,7 +3,7 @@
   <!-- This file makes it easy to include the settings for sensor managers -->
   <!-- This file makes it easy to include the settings for sensor managers -->
 
 
   <!-- Params for 3D sensors config -->
   <!-- Params for 3D sensors config -->
-  <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/sensors_3d.yaml" />
+  <rosparam command="load" file="$(find robot_config)/config/sensors_3d.yaml" />
 
 
   <!-- Params for the octomap monitor -->
   <!-- Params for the octomap monitor -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/setup_assistant.launch → ren_yuan/src/robot_config/launch/setup_assistant.launch

@@ -8,7 +8,7 @@
 
 
   <!-- Run -->
   <!-- Run -->
   <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
   <node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
-        args="--config_pkg=lstrobot_moveit_config_0605"
+        args="--config_pkg=robot_config"
         launch-prefix="$(arg launch_prefix)"
         launch-prefix="$(arg launch_prefix)"
         required="true"
         required="true"
         output="screen" />
         output="screen" />

+ 2 - 2
ren_yuan/src/lstrobot_moveit_config_0605/launch/simple_moveit_controller_manager.launch.xml → ren_yuan/src/robot_config/launch/simple_moveit_controller_manager.launch.xml

@@ -3,6 +3,6 @@
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
   <param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
 
 
   <!-- Load controller list to the parameter server -->
   <!-- Load controller list to the parameter server -->
-  <rosparam file="$(find lstrobot_moveit_config_0605)/config/simple_moveit_controllers.yaml" />
+  <rosparam file="$(find robot_config)/config/simple_moveit_controllers.yaml" />
-  <rosparam file="$(find lstrobot_moveit_config_0605)/config/ros_controllers.yaml" />
+  <rosparam file="$(find robot_config)/config/ros_controllers.yaml" />
 </launch>
 </launch>

+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/launch/stomp_planning_pipeline.launch.xml → ren_yuan/src/robot_config/launch/stomp_planning_pipeline.launch.xml

@@ -19,5 +19,5 @@
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
   <param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
 
 
-  <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/config/stomp_planning.yaml"/>
+  <rosparam command="load" file="$(find robot_config)/config/stomp_planning.yaml"/>
 </launch>
 </launch>

+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/trajectory_execution.launch.xml → ren_yuan/src/robot_config/launch/trajectory_execution.launch.xml


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/warehouse.launch → ren_yuan/src/robot_config/launch/warehouse.launch


+ 0 - 0
ren_yuan/src/lstrobot_moveit_config_0605/launch/warehouse_settings.launch.xml → ren_yuan/src/robot_config/launch/warehouse_settings.launch.xml


+ 1 - 1
ren_yuan/src/lstrobot_moveit_config_0605/package.xml → ren_yuan/src/robot_config/package.xml

@@ -1,6 +1,6 @@
 <package>
 <package>
 
 
-  <name>lstrobot_moveit_config_0605</name>
+  <name>robot_config</name>
   <version>0.3.0</version>
   <version>0.3.0</version>
   <description>
   <description>
      An automatically generated package with all the configuration and launch files for using the a9 with the MoveIt Motion Planning Framework
      An automatically generated package with all the configuration and launch files for using the a9 with the MoveIt Motion Planning Framework