renyuan

a9/urdf/a9.urdf

@@ -0,0 +1,465 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     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 -->
+<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">
+    <inertial>
+      <origin
+        xyz="-0.110121070288428 -0.0129999995820303 0.159000000973282"
+        rpy="0 0 0" />
+      <mass
+        value="0.0148298653353077" />
+      <inertia
+        ixx="2.02744416702812E-06"
+        ixy="-1.54215522606031E-15"
+        ixz="-3.37592192750916E-14"
+        iyy="1.40267326461625E-06"
+        iyz="-2.76126618174793E-15"
+        izz="1.40267288470545E-06" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="Link_1">
+    <inertial>
+      <origin
+        xyz="0.802922442229256 0.00984539192698494 1.19598646678643"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="0.19323050511757"
+        ixy="0.0386299594946521"
+        ixz="-0.388616965148905"
+        iyy="1.42567974975729"
+        iyz="0.0128991341474255"
+        izz="1.24513273855516" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.278"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="Link_1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.965"
+      upper="2.965"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_2">
+    <inertial>
+      <origin
+        xyz="0.117746157865896 0.0426453911640452 1.20871486266949"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="0.723493788556202"
+        ixy="0.0285843029943905"
+        ixz="-0.653933580723206"
+        iyy="1.42567974975729"
+        iyz="0.0290103266895879"
+        izz="0.714869455116528" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_2"
+    type="revolute">
+    <origin
+      xyz="0.15 -0.0328 0.172"
+      rpy="0 0.47052 0" />
+    <parent
+      link="Link_1" />
+    <child
+      link="Link_2" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.27"
+      upper="2.27"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_3">
+    <inertial>
+      <origin
+        xyz="0.00164806841162357 0.0743961632896021 -0.0125870623969895"
+        rpy="0 0 0" />
+      <mass
+        value="1.10722558532838" />
+      <inertia
+        ixx="0.00281804497282514"
+        ixy="1.0820613146504E-05"
+        ixz="-2.30905562044302E-05"
+        iyy="0.00127732014523064"
+        iyz="5.62797076557556E-06"
+        izz="0.0028367630953507" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_3"
+    type="revolute">
+    <origin
+      xyz="-0.59925 0 0.50037"
+      rpy="0 -0.28892 0" />
+    <parent
+      link="Link_2" />
+    <child
+      link="Link_3" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-4.0"
+      upper="4.0"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_4">
+    <inertial>
+      <origin
+        xyz="0.751501000756241 -0.00505225715113682 0.690060032582326"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="0.419099372956719"
+        ixy="-0.0348631653478693"
+        ixz="-0.581012917159635"
+        iyy="1.42564974048808"
+        iyz="-0.0218580250133829"
+        izz="1.01929387998522" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_4"
+    type="revolute">
+    <origin
+      xyz="-0.084223 0.0347 0.16884"
+      rpy="-3.119 -1.0471 -0.013042" />
+    <parent
+      link="Link_3" />
+    <child
+      link="Link_4" />
+    <axis
+      xyz="0.86603 0 0.5" />
+    <limit
+      lower="-3.49"
+      upper="3.49"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_5">
+    <inertial>
+      <origin
+        xyz="0.206551018322306 -0.00505225715114058 -0.104834036546721"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="0.745717188604768"
+        ixy="0.0281122476978264"
+        ixz="-0.653399595391099"
+        iyy="1.42564974048808"
+        iyz="0.030048545471202"
+        izz="0.69267606433717" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_5"
+    type="revolute">
+    <origin
+      xyz="0.92436 0 0.53588"
+      rpy="3.1416 -0.25867 3.1416" />
+    <parent
+      link="Link_4" />
+    <child
+      link="Link_5" />
+    <axis
+      xyz="0 1 0" />
+    <limit
+      lower="-2.49"
+      upper="2.49"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_6">
+    <inertial>
+      <origin
+        xyz="0.12280626304832 0.10284172733946 -0.0673247079199191"
+        rpy="0 0 0" />
+      <mass
+        value="8.19341168850207" />
+      <inertia
+        ixx="1.25065484857243"
+        ixy="-0.370837141231599"
+        ixz="-0.214061773475593"
+        iyy="0.581725625286335"
+        iyz="-0.544088624970368"
+        izz="1.03166251957125" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 0 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_6"
+    type="revolute">
+    <origin
+      xyz="0.037697 -0.0006997 -0.064094"
+      rpy="0.44032 0.19669 -0.83053" />
+    <parent
+      link="Link_5" />
+    <child
+      link="Link_6" />
+    <axis
+      xyz="0.49999 0 -0.86603" />
+    <limit
+      lower="-7.855"
+      upper="7.855"
+      effort="0"
+      velocity="0" />
+  </joint>
+  <link
+    name="Link_7">
+    <inertial>
+      <origin
+        xyz="0.0296425829147993 -6.47728578861279E-06 0.162281442392356"
+        rpy="0 0 0" />
+      <mass
+        value="7.07143232149748" />
+      <inertia
+        ixx="0.0103947066541271"
+        ixy="8.00167786886164E-07"
+        ixz="0.00131122028195651"
+        iyy="0.0112643862205598"
+        iyz="-2.38846633634112E-06"
+        izz="0.0065729384023248" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_7.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="package://a9/meshes/Link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_7"
+    type="fixed">
+    <origin
+      xyz="0 0 0"
+      rpy="-2.6972 0.28661 -2.1066" />
+    <parent
+      link="Link_6" />
+    <child
+      link="Link_7" />
+    <axis
+      xyz="0 0 0" />
+  </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>
+  
+</robot>

catkin_ws/.catkin_workspace

@@ -1 +0,0 @@
-# This file currently only serves to mark the location of a catkin workspace for tool integration

catkin_ws/src/CMakeLists.txt

@@ -1 +0,0 @@
-/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

catkin_ws/src/lstrobot_moveit_config_0605/config/sensors_3d.yaml

@@ -1,2 +0,0 @@
-sensors:
-  []

catkin_ws/src/lstrobot_planning/.vscode/c_cpp_properties.json

@@ -1,22 +0,0 @@
-{
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "${default}",
-        "limitSymbolsToIncludedHeaders": false
-      },
-      "includePath": [
-        "/home/robot/ROS/catkin_ws/devel/include/**",
-        "/opt/ros/noetic/include/**",
-        "/home/robot/ROS/COMP_URDF/src/moveit_demo/include/**",
-        "/usr/include/**"
-      ],
-      "name": "ROS",
-      "intelliSenseMode": "gcc-x64",
-      "compilerPath": "/usr/bin/gcc",
-      "cStandard": "gnu11",
-      "cppStandard": "c++14"
-    }
-  ],
-  "version": 4
-}

catkin_ws/src/lstrobot_planning/launch/moveit_control_server.launch

@@ -1,5 +0,0 @@
-<launch>
-	<node name="moveit_control_server" pkg="lstrobot_planning" type="moveitServer2.py" output="screen">
-   
-	</node>
-</launch>

catkin_ws/src/lstrobot_planning/launch/publish_pointcloud.launch

@@ -1,7 +0,0 @@
-<launch>
-  <!-- 启动publish_pointcloud节点 -->
-  <node pkg="lstrobot_planning" name="publish_pointcloud" type="dycl.py" required="true" output="screen">
-  </node>
-
-</launch>
-

catkin_ws/src/lstrobot_planning/launch/same_terminal_to_run.launch

@@ -1,7 +0,0 @@
-<launch>
-  <include file="$(find lstrobot_moveit_config_0605)/launch/demo.launch">
-  </include>
-  <node pkg="lstrobot_planning" name="set_update_paramter_p" type="set_update_paramter_p.py" required="true" output="screen">
-  </node>
-
-</launch>

catkin_ws/src/lstrobot_planning/launch/set_update_paramter_p.launch

@@ -1,5 +0,0 @@
-<launch>
-  <node pkg="lstrobot_planning" name="set_update_paramter_p" type="set_update_paramter_p.py" required="true" output="screen">
-  </node>
-
-</launch>

catkin_ws/src/lstrobot_planning/launch/start_ur5.launch

@@ -1,20 +0,0 @@
-<launch>
-  <arg name="limited" default="false" />
-  <arg name="config" default="true"/>
-
-  <!-- Launch ur5_bringup -->
-  <include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
-  </include>
-
-
-  <!-- Launch moveit -->
-  <include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
-    <arg name="limited" default="$(arg limited)"/>
-  </include>
-
-  <!--start rviz  -->
-  <include file="$(find ur5_moveit_config)/launch/moveit_rviz.launch">
-    <arg name="config" default="$(arg config)"/>
-  </include>
-
-</launch>

catkin_ws/src/lstrobot_planning/launch/test.launch

@@ -1,5 +0,0 @@
-<launch>
-  <node pkg="lstrobot_planning" name="speed" type="speed.py"  output="screen">
-  </node>
-
-</launch>

catkin_ws/src/lstrobot_planning/launch/ur5_bringup.launch

@@ -1,30 +0,0 @@
-<?xml version="1.0"?>
-<!--
-  Universal robot ur5 launch.  Loads ur5 robot description (see ur_common.launch
-  for more info)
-
-  Usage:
-    ur5_bringup.launch robot_ip:=<value>
--->
-<launch>
-  
-  <!-- robot_ip: IP-address of the robot's socket-messaging server -->
-  <arg name="robot_ip" default="192.168.50.100"/>
-  <arg name="limited" default="false"/>
-  <arg name="min_payload"  default="0.0"/>
-  <arg name="max_payload"  default="5.0"/>
-  <arg name="prefix" default="" />
-  <!-- robot model -->
-  <include file="$(find ur_description)/launch/ur5_upload.launch">
-    <arg name="limited" value="$(arg limited)"/>
-  </include>
-  
-  <!-- ur common -->
-  <include file="$(find ur_modern_driver)/launch/ur_common.launch">
-    <arg name="prefix"  value="$(arg prefix)" />
-    <arg name="robot_ip" value="$(arg robot_ip)"/>
-    <arg name="min_payload"  value="$(arg min_payload)"/>
-    <arg name="max_payload"  value="$(arg max_payload)"/>
-  </include>
-
-</launch>

catkin_ws/src/visual/.vscode/c_cpp_properties.json

@@ -1,22 +0,0 @@
-{
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "${default}",
-        "limitSymbolsToIncludedHeaders": false
-      },
-      "includePath": [
-        "/home/robot/ROS/catkin_ws/devel/include/**",
-        "/opt/ros/noetic/include/**",
-        "/home/robot/ROS/catkin_ws/src/visual/include/**",
-        "/usr/include/**"
-      ],
-      "name": "ROS",
-      "intelliSenseMode": "gcc-x64",
-      "compilerPath": "/usr/bin/gcc",
-      "cStandard": "gnu11",
-      "cppStandard": "c++14"
-    }
-  ],
-  "version": 4
-}

catkin_ws/src/visual/.vscode/settings.json

@@ -1,3 +0,0 @@
-{
-    "C_Cpp.errorSquiggles": "disabled"
-}

catkin_ws/src/lstrobot_planning/scripts/.vscode/c_cpp_properties.json → lst_robot_r/.vscode/c_cpp_properties.json

@@ -6,9 +6,13 @@
         "limitSymbolsToIncludedHeaders": false
       },
       "includePath": [
-        "/home/robot/ROS/catkin_ws/devel/include/**",
+        "/home/tong/catkin_ws/devel/include/**",
+        "/home/tong/moveit_ws/devel/include/**",
         "/opt/ros/noetic/include/**",
-        "/home/robot/ROS/catkin_ws/src/visual/include/**",
+        "/home/tong/catkin_ws/src/moveit_resources/prbt_ikfast_manipulator_plugin/include/**",
+        "/home/tong/catkin_ws/src/moveit_visual_tools/include/**",
+        "/home/tong/catkin_ws/src/rviz_visual_tools/include/**",
+        "/home/tong/moveit_ws/src/srdfdom/include/**",
         "/usr/include/**"
       ],
       "name": "ROS",

lst_robot_r/.vscode/settings.json

@@ -0,0 +1,12 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/tong/catkin_ws/devel/lib/python3/dist-packages",
+        "/home/tong/moveit_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/tong/catkin_ws/devel/lib/python3/dist-packages",
+        "/home/tong/moveit_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

catkin_ws/src/lstrobot_planning/CMakeLists.txt → lst_robot_r/CMakeLists.txt

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

lst_robot_r/launch/start.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node pkg="lst_robot_r" name="start" type="start.py" required="true" output="screen">
+  </node>
+
+</launch>

catkin_ws/src/lstrobot_planning/package.xml → lst_robot_r/package.xml

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>lstrobot_planning</name>
+  <name>lst_robot_r</name>
   <version>0.0.0</version>
-  <description>The lstrobot_planning package</description>
+  <description>The lst_robot_r package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->

catkin_ws/src/lstrobot_planning/scripts/command.py → lst_robot_r/scripts/command.py

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

catkin_ws/src/lstrobot_planning/scripts/dycl_0506.py → lst_robot_r/scripts/dycl_0506.py

@@ -82,7 +82,7 @@ def load_point_cloud_from_binary_txt(file_path):
 
 pcd = o3d.geometry.PointCloud()
 pcd.points = o3d.utility.Vector3dVector(load_point_cloud_from_binary_txt(file_path))
-pcd = pcd.voxel_down_sample(voxel_size = 4)
+#pcd = pcd.voxel_down_sample(voxel_size = 4)
 
 ################################去掉焊缝周围的点云#################################
 # 计算焊缝向量
@@ -112,7 +112,7 @@ ptCloud_scaled2.points = o3d.utility.Vector3dVector(data_scaled2)
 def build_pointcloud2_msg(points):
     msg = PointCloud2()
     msg.header.stamp = rospy.Time(0)
-    msg.header.frame_id = "base_link"
+    msg.header.frame_id = "world"
 
     if len(points.shape) == 3:
         msg.height = points.shape[1]

catkin_ws/src/lstrobot_planning/scripts/hanqiangpose.py → lst_robot_r/scripts/hanqiangpose.py

@@ -30,7 +30,7 @@ def get_hanfeng(file_path):
             point1_str = points_str[0].split(',')
             point2_str = points_str[1].split(',')
             
-            # 转换字符串为浮点数列表，构造三维点
+            # 转换字符串为浮点数列表，构造三维点，使用列表推导式
             point1 = [float(coord) for coord in point1_str]
             point2 = [float(coord) for coord in point2_str]
 
@@ -67,12 +67,12 @@ def run():
     # 计算位姿
     for i in range(len(hanfeng)): 
         if(calculate_angle_with_xy_plane(start_points[i], end_points[i])<45):#平缝
-            q1 = rpy2quaternion(0,0,90)
-            q2 = rpy2quaternion(0,0,90)
+            q1 = rpy2quaternion(0,0,3*np.pi/4)    #ping
+            q2 = rpy2quaternion(0,0,3*np.pi/4)
             result.append(("s", start_points[i], end_points[i], q1, q2))
         else:
-            q1 = rpy2quaternion(0,0,90)
-            q2 = rpy2quaternion(0,0,90)
+            q1 = rpy2quaternion(0,np.pi/4,0)      #shu
+            q2 = rpy2quaternion(0,np.pi/4,0)
             result.append(("s", start_points[i], end_points[i], q1, q2))
 
 
@@ -86,4 +86,5 @@ def run():
     rospy.loginfo("Welding Angle calculation completed")
 if __name__ == '__main__':     
     
-    run()
+    run()
+    

catkin_ws/src/lstrobot_planning/scripts/hjsx.py → lst_robot_r/scripts/hjsx.py

@@ -2,6 +2,7 @@ import numpy as np
 import os
 import rospy
 from math import acos, degrees
+
 def calculate_distance(point1, point2):
     return np.linalg.norm(np.array(point1) - np.array(point2))
 
@@ -49,7 +50,7 @@ def get_hanfeng(file_path):
 
         angle = calculate_angle_with_xy_plane(point1,point2)
         
-        if abs(angle) < 30:
+        if abs(angle) < 30:     #与xy平面夹角小于30度
             data_ping.append([point1, point2, flag_sequence])
         else:
             data_shu.append([*sorted([point1, point2], key=lambda p: p[2]), flag_sequence]) 
@@ -84,8 +85,8 @@ def sort_welds_by_distance_ping(data_ping, reference_point):
         data_with_distances.sort(key=lambda x: x[0])
 
         # 初始化结果和当前终点
-        sorted_data_ping.append(data_with_distances.pop(0)[-3:])
-        reference_point = sorted_data_ping[-1][1]
+        sorted_data_ping.append(data_with_distances.pop(0)[-3:])   
+        reference_point = sorted_data_ping[-1][1]                   
         data_ping = [[start, end,flag_sequence] for distances1,distances2, start,end,flag_sequence in data_with_distances]
     return sorted_data_ping
 def sort_welds_by_distance_shu(data_shu, reference_point):

catkin_ws/src/lstrobot_planning/scripts/moveitServer2.py → lst_robot_r/scripts/moveitServer2.py

@@ -63,18 +63,19 @@ class MoveIt_Control:
         #self.move_j()
         self.go_home()
         self.home_pose=self.get_now_pose()
+        #self.go_ready()
 
-    
+    #TODO plan_cartesian_path
     def plan_cartesian_path_LLL(self,point_s,point_e,waypoints,trajectory,trajectory_with_type,a=1,v=1):
         fraction = 0.0  # 路径规划覆盖率
         maxtries = 50  # 最大尝试规划次数
         attempts = 0  # 已经尝试规划次数
 
         #起点位置设置为规划组最后一个点
-        pose = self.arm.get_current_pose(self.end_effector_link).pose
-        wpose = deepcopy(pose)
+        start_pose = self.arm.get_current_pose(self.end_effector_link).pose
+        wpose = deepcopy(start_pose)
         waypoint=[]
-        print(waypoint)
+        #print(waypoint)
         if waypoints:
             wpose.position.x=waypoints[-1].position.x
             wpose.position.y=waypoints[-1].position.y
@@ -84,16 +85,6 @@ class MoveIt_Control:
             wpose.orientation.z=waypoints[-1].orientation.z
             wpose.orientation.w=waypoints[-1].orientation.w
             waypoint.append(deepcopy(wpose))
-        # else:
-        #     wpose.position.x=self.home_pose[0]
-        #     wpose.position.y=self.home_pose[1]
-        #     wpose.position.z=self.home_pose[2]
-        #     wpose.orientation.x=self.home_pose[3]
-        #     wpose.orientation.y=self.home_pose[4]
-        #     wpose.orientation.z=self.home_pose[5]
-        #     wpose.orientation.w=self.home_pose[6]
-        #     waypoint.append(deepcopy(wpose))
-            #waypoints.append(deepcopy(wpose))
 
         #wpose = deepcopy(pose)
         wpose.position.x=point_s[0]
@@ -121,19 +112,23 @@ class MoveIt_Control:
             (plan, fraction) = self.arm.compute_cartesian_path(
                 waypoint,  # waypoint poses，路点列表
                 0.001,  # eef_step，终端步进值
-                0.0,  # jump_threshold，跳跃阈值
-                True)  # avoid_collisions，避障规划
+                #0.0,  # jump_threshold，跳跃阈值
+                True,
+                None)  # avoid_collisions，避障规划
             attempts += 1
 
         if fraction == 1.0 :
             rospy.loginfo("Path computed successfully.")
             #traj = plan
             trajj = plan #取出规划的信息
+            retimed_planed=self.retime_plan(trajj)
             trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
             trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"  
             trajj_with_type.points[-1].type = "end"
             trajectory.append(trajj)
-            moveit_server.arm.execute(trajj)
+            rospy.loginfo("开始执行")
+            moveit_server.arm.execute(retimed_planed)
+            rospy.loginfo("执行结束")
             trajectory_with_type.append(trajj_with_type)
         else:
             rospy.loginfo(
@@ -147,7 +142,18 @@ class MoveIt_Control:
         #self.hf_num=self.hf_num+1#焊缝计数
         return waypoints,trajectory,trajectory_with_type
     
+    def retime_plan(self,traj):
+        current_state = self.arm.get_current_state()
+        
+        retimed_traj = self.arm.retime_trajectory(
+            current_state,
+            traj,
+            velocity_scaling_factor=0.3,
+            acceleration_scaling_factor=1)
+        
+        return retimed_traj
 
+    
     def get_now_pose(self):
         # 获取机械臂当前位姿
         current_pose = self.arm.get_current_pose(self.end_effector_link).pose
@@ -272,8 +278,6 @@ class MoveIt_Control:
         return points,trajectory,trajectory_with_type,er
     
 
-
-
     #TODO go_home
     @decorator_time  
     def go_home(self,a=1,v=1):
@@ -285,9 +289,20 @@ class MoveIt_Control:
         self.arm.go()
         rospy.loginfo("go_home end")
         # rospy.sleep(1)
+        
+    def go_ready(self,a=1,v=1):
+        rospy.loginfo("go_ready start")
+        self.arm.set_max_acceleration_scaling_factor(a)
+        self.arm.set_max_velocity_scaling_factor(v)
+        self.arm.set_named_target('ready')
+        rospy.loginfo("get_ready end")
+        self.arm.go()
+        rospy.loginfo("go_ready end")
     
     #TODO go_home_justplan
+    @decorator_time
     def go_home_justplan(self,trajectory,trajectory_with_type,a=1,v=1):
+        rospy.loginfo("go_home start")
         self.arm.set_max_acceleration_scaling_factor(a)
         self.arm.set_max_velocity_scaling_factor(v)
         state = self.arm.get_current_state()
@@ -296,6 +311,8 @@ class MoveIt_Control:
         self.arm.set_named_target('home')
         traj = self.arm.plan()
         trajj = traj[1]
+        moveit_server.arm.execute(trajj)
+        rospy.loginfo("go_home end")
         traj_with_type = mark_the_traj(trajj,"go-home",welding_sequence)
         trajectory.append(trajj)
         trajectory_with_type.append(traj_with_type)
@@ -321,13 +338,14 @@ class MoveIt_Control:
 
             rospy.loginfo("第%d条焊缝规划完毕",i+1)
             rospy.loginfo("*******************")
-        #if gohome:
-            #points,trajectory,trajectory_with_type,err= self.move_p_flexible(self.home_pose,points,trajectory,trajectory_with_type,v=speed_v)
-            #trajectory,trajectory_with_type = self.go_home_justplan(trajectory,trajectory_with_type)
+        rospy.loginfo("All of The trajectory Plan successfully")
+        rospy.loginfo("*******************")
+        if gohome:
+            #trajectory,trajectory_with_type= self.move_p_flexible(trajectory,trajectory_with_type)
+            trajectory,trajectory_with_type = self.go_home_justplan(trajectory,trajectory_with_type)
 
         traj_merge= merge_trajectories(trajectory)
         trajectory_with_type_merge= merge_trajectories_with_type(trajectory_with_type)
-        rospy.loginfo("All of The trajectory Plan successfully")
         return trajectory,traj_merge,trajectory_with_type_merge
 
 ###########################################################class end#############################################################
@@ -365,7 +383,6 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
     ]
     return traj_with_type
 
-
 def merge_trajectories(trajectories):
     if not trajectories:
         return None
@@ -482,12 +499,9 @@ def get_user_input():
         #rospy.loginfo(f"用户输入的速度缩放因子为{vv}")
         return vv
     except Exception as e:
-        rospy.logerr(f"发生错误：{e}")
+        rospy.logerr(f"发生错误：{e}") 
         
 
-            
-
-
 if __name__ =="__main__":
     # from redis_publisher import Publisher
     # publisher = Publisher()
@@ -502,9 +516,10 @@ if __name__ =="__main__":
     # rospy.loginfo(f"用户输入的速度缩放因子为{speed_v}")
 
     trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
-    #ROS2_msgs(trajectory_with_type_merge,moveit_server)
-    #write_ee(trajectory_with_type_merge, moveit_server)
-    # moveit_server.go_home()
-    # moveit_server.arm.execute(trajectory_merge)
+
+    #moveit_server.go_ready()
+    rospy.loginfo("开始执行合并后轨迹")
+    moveit_server.arm.execute(trajectory_merge)
+    rospy.loginfo("合并后轨迹执行完毕")
     
     rospy.set_param("sign_control",0)

lst_robot_r/scripts/pcd2binary.py

@@ -0,0 +1,24 @@
+import open3d as o3d
+import numpy as np
+
+
+def pcd_to_binary(pcd_file, output_file):
+    pcd = o3d.io.read_point_cloud(pcd_file)
+    points = np.asarray(pcd.points)
+    binary_data = points[:, :3]
+    with open(output_file, 'wb') as f:
+        f.write(binary_data.tobytes())
+
+
+# def binary_to_nparray(binary_file, output_file):
+#     with open(binary_file, 'rb') as f:
+#         binary_data = f.read()
+#     # 将二进制数据转换为 NumPy 数组
+#     point_cloud = np.frombuffer(binary_data, dtype=np.float64).reshape(-1, 3)
+#     np.savetxt(output_file, point_cloud)
+
+
+if __name__ == '__main__':
+    pcd_to_binary(r'/home/tong/renyuan_pointcloud/4/trans/trans.pcd', r'/home/tong/renyuan_pointcloud/4/trans/pointcloud.txt')
+    #pcd_to_binary(r'/home/tong/renyuan_pointcloud/3/trans/transformed_point_cloud.pcd', r'/home/tong/renyuan_pointcloud/3/trans/pointcloud.txt')
+    #binary_to_nparray(r'./pointcloud.txt', r'./111123123123.txt')

lst_robot_r/scripts/rotation_pcd.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+
+import rospy
+import pcl
+import numpy as np
+
+def main():
+    # 初始化ROS节点
+    rospy.init_node('transform_pcd_node', anonymous=True)
+
+    # 定义点云地图的路径和文件名
+    pcd_path = "/home/tong/renyuan_pointcloud/4/trans/LazerData.pcd"
+
+    # 读取点云地图
+    cloud = pcl.load(pcd_path)
+    
+    points = cloud.to_array()
+    centroid = np.mean(points, axis=0)
+    points_centered = points - centroid
+    
+    theta = np.pi
+    
+    #旋转点云
+    transform_matrix = np.array([[1,  0,          0],
+                                 [0, np.cos(theta), -np.sin(theta)],
+                                 [0, np.sin(theta),  np.cos(theta)]], dtype=np.float32)
+
+
+
+    # 手动变换点云
+    transformed_points = np.dot(points_centered[:, :3],transform_matrix.T)
+    transformed_points += centroid
+    
+    # #定义位移向量(沿Z轴方向移动一个单位长度)
+    
+    # print("Transformed Points (before translation):", transformed_points)
+    
+    translation_vector = np.array([-1000, 0, 0], dtype=np.float32)
+    transformed_points += translation_vector
+    
+    # print("Transformed Points (after translation):", transformed_points)
+    
+    # 创建新的点云对象
+    transformed_cloud = pcl.PointCloud()
+    transformed_cloud.from_array(transformed_points.astype(np.float32))
+
+    # 保存旋转后的点云地图到PCD文件中
+    pcl.save(transformed_cloud, "/home/tong/renyuan_pointcloud/4/trans/trans.pcd")
+
+if __name__ == "__main__":
+    main()
+

catkin_ws/src/lstrobot_planning/scripts/set_update_paramter_p.py → lst_robot_r/scripts/start.py

@@ -46,26 +46,11 @@ if __name__ == "__main__":
 
     rospy.set_param("sign_control",1)
     rospy.set_param("sign_pointcloud",0)
-    #rospy.set_param("sign_traj_accepted",0)
 
-    rospy.set_param("yaw",np.pi/6)  #内收角（偏航角）
-    rospy.set_param("yaw_rate",100) #偏航角过渡速度（角度变化45度/100mm）
-    rospy.set_param("pitch_of_Horizontalweld",np.pi/4)  #和底面夹角（俯仰角）
-    rospy.set_param("pitch_of_Verticalweld",np.pi/5)
+    rospy.set_param("folder_path","/home/tong/moveit_ws/data/3")
+    #rospy.set_param("folder_path","/home/tong/moveit_ws/data/renyuan_2")
 
-    # rospy.set_param("yaw",0)  #内收角（偏航角）
-    # rospy.set_param("yaw_rate",100) #偏航角过渡速度（角度变化45度/100mm）
-    # rospy.set_param("pitch_of_Horizontalweld",0)  #和底面夹角（俯仰角）
-    # rospy.set_param("pitch_of_Verticalweld",0)
-
-    #rospy.set_param("culling_radius",0) #焊缝剔除半径
-    rospy.set_param("folder_path","/home/robot/ROS/catkin_ws/data/2")
-
-    rospy.loginfo("当前参数值:")# 显示参数当前值
-    rospy.loginfo("sign_pointcloud = %s", rospy.get_param("sign_pointcloud"))
-    rospy.loginfo("yaw = %s",rospy.get_param("yaw")/np.pi*180)
-    rospy.loginfo("pitch_of_Horizontalweld = %s",rospy.get_param("pitch_of_Horizontalweld")/np.pi*180)
-    rospy.loginfo("pitch_of_Verticalweld = %s",rospy.get_param("pitch_of_Verticalweld")/np.pi*180)
+   
     rospy.loginfo("等待rviz启动")
     wait_for_topic("/execute_trajectory/status",actionlib_msgs.msg.GoalStatusArray)
     
@@ -84,7 +69,7 @@ if __name__ == "__main__":
                 if(aaa =='1' or aaa=='2'):
                     rospy.set_param("sign_control",aaa)
                 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)
                 waited_once = True
         elif sign_control == "1":
@@ -94,16 +79,18 @@ if __name__ == "__main__":
             # 清除场景
             clear_octomap()
             #点云计算并发布
-            # process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
-            # process.start()
+            #process = multiprocessing.Process(target=command.launch_publish_pointcloud_background)
+            #process.start()
+            #input("请在rviz中查看点云，然后按回车键继续...")
             #计算焊接顺序和焊接姿态
             hjsx.run()
             hanqiangpose.run()
+            time.sleep(10)
             command.load_visual()
             # 等待 /move_group/monitored_planning_scene 话题发布消息
-            #rospy.loginfo("等待场景地图加载完毕...")
-            #wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
-            #rospy.loginfo(f"场景地图已加载完毕")
+            # rospy.loginfo("等待场景地图加载完毕...")
+            # wait_for_topic('/move_group/monitored_planning_scene', PlanningScene)
+            # rospy.loginfo(f"场景地图已加载完毕")
             rospy.set_param("sign_pointcloud",1)
             #rospy.loginfo("终止点云发布，关闭发布窗口")
             

catkin_ws/src/lstrobot_moveit_config_0605/.setup_assistant → robot_config/.setup_assistant

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

catkin_ws/src/lstrobot_moveit_config_0605/CMakeLists.txt → robot_config/CMakeLists.txt

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

catkin_ws/src/lstrobot_moveit_config_0605/config/a9.srdf → robot_config/config/a9.srdf

@@ -10,9 +10,17 @@
     <!--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-->
     <group name="manipulator">
-        <chain base_link="base_link" tip_link="end_link_now"/>
+        <chain base_link="world" tip_link="end_link_now"/>
     </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_state name="home" group="manipulator">
+        <joint name="joint_1" value="0"/>
+        <joint name="joint_2" value="-1.1311"/>
+        <joint name="joint_3" value="1.459"/>
+        <joint name="joint_4" value="-3.158"/>
+        <joint name="joint_5" value="-0.258"/>
+        <joint name="joint_6" value="-0.022"/>
+    </group_state>
     <group_state name="zero" group="manipulator">
         <joint name="joint_1" value="0"/>
         <joint name="joint_2" value="0"/>
@@ -21,13 +29,13 @@
         <joint name="joint_5" value="0"/>
         <joint name="joint_6" value="0"/>
     </group_state>
-    <group_state name="home" group="manipulator">
+    <group_state name="ready" group="manipulator">
         <joint name="joint_1" value="0"/>
-        <joint name="joint_2" value="-1.1311"/>
+        <joint name="joint_2" value="-1.278"/>
         <joint name="joint_3" value="1.459"/>
         <joint name="joint_4" value="0"/>
-        <joint name="joint_5" value="2.8033"/>
-        <joint name="joint_6" value="2.6393"/>
+        <joint name="joint_5" value="2.490"/>
+        <joint name="joint_6" value="3.102"/>
     </group_state>
     <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
     <disable_collisions link1="Link_1" link2="Link_2" reason="Adjacent"/>

catkin_ws/src/lstrobot_moveit_config_0605/config/chomp_planning.yaml → robot_config/config/chomp_planning.yaml

@@ -16,3 +16,4 @@ collision_threshold: 0.07
 use_stochastic_descent: true
 enable_failure_recovery: false
 max_recovery_attempts: 5
+trajectory_initialization_method: "fillTrajectory"

catkin_ws/src/lstrobot_moveit_config_0605/config/fake_controllers.yaml → robot_config/config/fake_controllers.yaml

@@ -10,4 +10,4 @@ controller_list:
       - joint_6
 initial:  # Define initial robot poses per group
   - group: manipulator
-    pose: zero
+    pose: home

catkin_ws/src/lstrobot_moveit_config_0605/config/gazebo_a9.urdf → robot_config/config/gazebo_a9.urdf

@@ -3,6 +3,12 @@
      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 -->
 <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">
         <inertial>
             <origin xyz="-0.110121070288428 -0.0129999995820303 0.159000000973282" rpy="0 0 0" />
@@ -52,7 +58,7 @@
         <parent link="base_link" />
         <child link="Link_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>
     <link name="Link_2">
         <inertial>
@@ -81,7 +87,7 @@
         <parent link="Link_1" />
         <child link="Link_2" />
         <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>
     <link name="Link_3">
         <inertial>
@@ -110,7 +116,7 @@
         <parent link="Link_2" />
         <child link="Link_3" />
         <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>
     <link name="Link_4">
         <inertial>
@@ -139,7 +145,7 @@
         <parent link="Link_3" />
         <child link="Link_4" />
         <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>
     <link name="Link_5">
         <inertial>
@@ -168,7 +174,7 @@
         <parent link="Link_4" />
         <child link="Link_5" />
         <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>
     <link name="Link_6">
         <inertial>
@@ -197,7 +203,7 @@
         <parent link="Link_5" />
         <child link="Link_6" />
         <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>
     <link name="Link_7">
         <inertial>
@@ -227,6 +233,12 @@
         <child link="Link_7" />
         <axis xyz="0 0 0" />
     </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">
         <type>transmission_interface/SimpleTransmission</type>
         <joint name="joint_1">

catkin_ws/src/lstrobot_moveit_config_0605/config/ompl_planning.yaml → robot_config/config/ompl_planning.yaml

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

robot_config/config/sensors_3d.yaml

@@ -0,0 +1,9 @@
+sensors:
+  - 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/output
+    point_subsample: 1
+    sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater

catkin_ws/src/lstrobot_moveit_config_0605/launch/chomp_planning_pipeline.launch.xml → 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="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>

catkin_ws/src/lstrobot_moveit_config_0605/launch/default_warehouse_db.launch → robot_config/launch/default_warehouse_db.launch

@@ -2,7 +2,7 @@
 
   <arg name="reset" default="false"/>
   <!-- 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 -->
   <include file="$(dirname)/warehouse.launch">

catkin_ws/src/lstrobot_moveit_config_0605/launch/demo.launch → robot_config/launch/demo.launch

@@ -6,7 +6,7 @@
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
   <!-- 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 -->
   <arg name="debug" default="false" />

catkin_ws/src/lstrobot_moveit_config_0605/launch/fake_moveit_controller_manager.launch.xml → robot_config/launch/fake_moveit_controller_manager.launch.xml

@@ -7,6 +7,6 @@
   <param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
 
   <!-- 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>

catkin_ws/src/lstrobot_moveit_config_0605/launch/gazebo.launch → robot_config/launch/gazebo.launch

@@ -13,7 +13,7 @@
   </include>
 
   <!-- 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 -->
   <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
@@ -23,7 +23,7 @@
     respawn="false" output="screen" />
 
   <!-- 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"/>
 
   <!-- Spawn the Gazebo ROS controllers -->

catkin_ws/src/lstrobot_moveit_config_0605/launch/moveit.rviz → robot_config/launch/moveit.rviz

@@ -4,10 +4,17 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
-        - /Axes1
-        - /TF1
+        - /Global Options1
+        - /Grid1
+        - /MotionPlanning1
+        - /MotionPlanning1/Scene Geometry1
+        - /MotionPlanning1/Planning Request1
+        - /RobotModel1
+        - /RobotModel1/Links1
+        - /MarkerArray1
+        - /MarkerArray1/Status1
       Splitter Ratio: 0.5
-    Tree Height: 808
+    Tree Height: 356
   - Class: rviz/Help
     Name: Help
   - Class: rviz/Views
@@ -219,14 +226,6 @@ Visualization Manager:
         Show Robot Visual: false
       Value: true
       Velocity_Scaling_Factor: 0.1
-    - Class: rviz/MarkerArray
-      Enabled: true
-      Marker Topic: rviz_visual_tools
-      Name: MarkerArray
-      Namespaces:
-        {}
-      Queue Size: 100
-      Value: true
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -291,42 +290,34 @@ Visualization Manager:
       Update Interval: 0
       Value: true
       Visual Enabled: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: rviz_visual_tools
+      Name: MarkerArray
+      Namespaces:
+        Path: true
+        Text: true
+        point0  0.072094  -0.030473  0.455566: true
+        point0  0.073125  0.041527  0.449824: true
+        point1  0.109523  0.042320  0.457250: true
+      Queue Size: 100
+      Value: true
     - Alpha: 1
       Class: rviz/Axes
       Enabled: true
-      Length: 0.05000000074505806
+      Length: 0.054999999701976776
       Name: Axes
       Radius: 0.009999999776482582
-      Reference Frame: Link_7
+      Reference Frame: end_link_now
       Show Trail: false
       Value: true
     - Class: rviz/TF
-      Enabled: true
+      Enabled: false
       Filter (blacklist): ""
       Filter (whitelist): ""
       Frame Timeout: 15
       Frames:
-        All Enabled: false
-        Link_1:
-          Value: true
-        Link_2:
-          Value: true
-        Link_3:
-          Value: true
-        Link_4:
-          Value: true
-        Link_5:
-          Value: false
-        Link_6:
-          Value: false
-        Link_7:
-          Value: true
-        base_link:
-          Value: true
-        end_link_now:
-          Value: true
-        world:
-          Value: true
+        All Enabled: true
       Marker Alpha: 1
       Marker Scale: 1
       Name: TF
@@ -334,19 +325,9 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
-        world:
-          base_link:
-            Link_1:
-              Link_2:
-                Link_3:
-                  Link_4:
-                    Link_5:
-                      Link_6:
-                        Link_7:
-                          end_link_now:
-                            {}
+        {}
       Update Interval: 0
-      Value: true
+      Value: false
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -363,7 +344,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.121934413909912
+      Distance: 2.6588308811187744
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -371,22 +352,22 @@ Visualization Manager:
         Value: false
       Field of View: 0.75
       Focal Point:
-        X: -0.9589470028877258
-        Y: -0.3118433952331543
-        Z: -1.5461976528167725
+        X: -0.715438961982727
+        Y: -0.06119576096534729
+        Z: 1.1657675504684448
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6150007843971252
-      Target Frame: base_link
-      Yaw: 1.7799382209777832
+      Pitch: 0.4800015091896057
+      Target Frame: world
+      Yaw: 3.261983871459961
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1043
+  Height: 1018
   Help:
     collapsed: false
   Hide Left Dock: false
@@ -395,9 +376,9 @@ Window Geometry:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003b9fc0200000007fb000000100044006900730070006c006100790073010000003d000003b9000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000001880000017d00ffffff00000587000003b900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000001f3000003a0fc0200000007fb000000100044006900730070006c006100790073010000003d000001f5000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000238000001a50000017d00ffffff00000587000003a000000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1920
   X: 0
-  Y: 0
+  Y: 28

catkin_ws/src/lstrobot_moveit_config_0605/launch/ompl-chomp_planning_pipeline.launch.xml → robot_config/launch/ompl-chomp_planning_pipeline.launch.xml

@@ -1,6 +1,6 @@
 <launch>
   <!-- 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"
          default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
                   default_planner_request_adapters/AddTimeParameterization
@@ -13,7 +13,7 @@
   </include>
 
   <!-- 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 -->
   <param name="trajectory_initialization_method" value="fillTrajectory"/>

catkin_ws/src/lstrobot_moveit_config_0605/launch/ompl_planning_pipeline.launch.xml → 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="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>

catkin_ws/src/lstrobot_moveit_config_0605/launch/planning_context.launch → 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"/>
 
   <!-- 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) -->
   <group ns="$(arg robot_description)_planning">
-    <rosparam command="load" file="$(find lstrobot_moveit_config_0605)/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/joint_limits.yaml"/>
+    <rosparam command="load" file="$(find robot_config)/config/cartesian_limits.yaml"/>
   </group>
 
   <!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
   <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>
 

catkin_ws/src/lstrobot_moveit_config_0605/launch/ros_controllers.launch → robot_config/launch/ros_controllers.launch

@@ -2,7 +2,7 @@
 <launch>
 
   <!-- 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 -->
   <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"

catkin_ws/src/lstrobot_moveit_config_0605/launch/run_benchmark_ompl.launch → robot_config/launch/run_benchmark_ompl.launch

@@ -15,7 +15,7 @@
 
   <!-- 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">
-    <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>
 
 </launch>

catkin_ws/src/lstrobot_moveit_config_0605/launch/sensor_manager.launch.xml → robot_config/launch/sensor_manager.launch.xml

@@ -3,11 +3,11 @@
   <!-- This file makes it easy to include the settings for sensor managers -->
 
   <!-- 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 -->
   <!--  <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
-  <param name="octomap_resolution" type="double" value="0.025" />
+  <param name="octomap_resolution" type="double" value="0.002" />
   <param name="max_range" type="double" value="5.0" />
 
   <!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->

catkin_ws/src/lstrobot_moveit_config_0605/launch/setup_assistant.launch → robot_config/launch/setup_assistant.launch

@@ -8,7 +8,7 @@
 
   <!-- Run -->
   <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)"
         required="true"
         output="screen" />

catkin_ws/src/lstrobot_moveit_config_0605/launch/simple_moveit_controller_manager.launch.xml → robot_config/launch/simple_moveit_controller_manager.launch.xml

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

catkin_ws/src/lstrobot_moveit_config_0605/launch/stomp_planning_pipeline.launch.xml → 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="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>

catkin_ws/src/lstrobot_moveit_config_0605/package.xml → robot_config/package.xml

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

catkin_ws/src/visual/CMakeLists.txt → visual/CMakeLists.txt

@@ -5,7 +5,7 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
-  lstrobot_moveit_config_0605
+  robot_config
 )
 
 catkin_package(