+perception +gripper _control

mrceki · Feb 17, 2023 · bad2e67 · bad2e67
1 parent 2243df1
commit bad2e67
Show file tree

Hide file tree

Showing 133 changed files with 2,322 additions and 1,483 deletions.
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
@@ -1,37 +1,38 @@
 {
-  "configurations": [
-    {
-      "browse": {
-        "databaseFilename": "${default}",
-        "limitSymbolsToIncludedHeaders": false
-      },
-      "includePath": [
-        "/home/cenk/catkin_ws/devel/include/**",
-        "/opt/ros/noetic/include/**",
-        "/home/cenk/catkin_ws/src/air_hardware_interface/include/**",
-        "/home/cenk/catkin_ws/src/air_moveit_config/include/**",
-        "/home/cenk/catkin_ws/src/moveit_task_constructor/core/include/**",
-        "/home/cenk/catkin_ws/src/moveit_task_constructor/demo/include/**",
-        "/home/cenk/catkin_ws/src/octomap_mapping/octomap_server/include/**",
-        "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_control/include/**",
-        "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_driver/include/**",
-        "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_gazebo/include/**",
-        "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_hardware_interface/include/**",
-        "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_msgs/include/**",
-        "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_srvs/include/**",
-        "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_utils/include/**",
-        "/home/cenk/catkin_ws/src/qbhand-ros/qb_hand_gazebo/include/**",
-        "/home/cenk/catkin_ws/src/qbhand-ros/qb_hand_hardware_interface/include/**",
-        "/home/cenk/catkin_ws/src/realsense-ros/realsense2_camera/include/**",
-        "/home/cenk/catkin_ws/src/moveit_task_constructor/rviz_marker_tools/include/**",
-        "/usr/include/**"
-      ],
-      "name": "ROS",
-      "intelliSenseMode": "gcc-x64",
-      "compilerPath": "/usr/bin/gcc",
-      "cStandard": "gnu11",
-      "cppStandard": "c++14"
-    }
-  ],
-  "version": 4
+    "configurations": [
+        {
+            "browse": {
+                "databaseFilename": "${default}",
+                "limitSymbolsToIncludedHeaders": false
+            },
+            "includePath": [
+                "/home/cenk/catkin_ws/devel/include/**",
+                "/opt/ros/noetic/include/**",
+                "/home/cenk/catkin_ws/src/air_hardware_interface/include/**",
+                "/home/cenk/catkin_ws/src/air_moveit_config/include/**",
+                "/home/cenk/catkin_ws/src/moveit_task_constructor/core/include/**",
+                "/home/cenk/catkin_ws/src/moveit_task_constructor/demo/include/**",
+                "/home/cenk/catkin_ws/src/octomap_mapping/octomap_server/include/**",
+                "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_control/include/**",
+                "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_driver/include/**",
+                "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_gazebo/include/**",
+                "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_hardware_interface/include/**",
+                "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_msgs/include/**",
+                "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_srvs/include/**",
+                "/home/cenk/catkin_ws/src/qbdevice-ros/qb_device_utils/include/**",
+                "/home/cenk/catkin_ws/src/qbhand-ros/qb_hand_gazebo/include/**",
+                "/home/cenk/catkin_ws/src/qbhand-ros/qb_hand_hardware_interface/include/**",
+                "/home/cenk/catkin_ws/src/realsense-ros/realsense2_camera/include/**",
+                "/home/cenk/catkin_ws/src/moveit_task_constructor/rviz_marker_tools/include/**",
+                "/usr/include/**",
+                "/usr/include/pcl-1.10"
+            ],
+            "name": "ROS",
+            "intelliSenseMode": "gcc-x64",
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "gnu11",
+            "cppStandard": "c++14"
+        }
+    ],
+    "version": 4
 }
diff --git a/.vscode/settings.json b/.vscode/settings.json
@@ -3,5 +3,85 @@
     "python.autoComplete.extraPaths": [
         "/home/cenk/catkin_ws/devel/lib/python3/dist-packages",
         "/opt/ros/noetic/lib/python3/dist-packages"
-    ]
+    ],
+    "python.analysis.extraPaths": [
+        "/home/cenk/catkin_ws/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "any": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "strstream": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "chrono": "cpp",
+        "codecvt": "cpp",
+        "complex": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "filesystem": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "map": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "regex": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cfenv": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp",
+        "bit": "cpp"
+    },
+    "C_Cpp.errorSquiggles": "disabled"
 }
diff --git a/README.md b/README.md
@@ -1,17 +1,48 @@
 #  Altınay Air
 
-## Octomap
+## Air Robot Bringup
+To bring up the robot, you can use these command lines:
+### Real Hardware
 ```
-roslaunch air_moveit_config air_cloud.launch
+roslaunch air_moveit_config air_bringup.launch controller:=ros_control
 ```
+### Fake Hardware 
+```
+roslaunch air_moveit_config air_bringup.launch
+```
+Add to command line `bagfile:=true` for working with rosbag(camera).
 
-## MoveIt Task Constructor
+## Air Robot Launch Files
+If you want to run launch files in different terminals use these command lines:
+
+### Air Arm - MoveIt - Hardware Interface
+This launch file runs MoveIt and hardware interface.
 ```
-roslaunch air_moveit_config air_mtc.launch
+roslaunch air_moveit_config demo_ros_control.launch controller:=ros_control
+```
+### Realsense Camera
+This launch files runs Realsense Camera and nodes.
+```
+roslaunch realsense2_camera rs_camera.launch
+```
+### Object Detection 
+This launch files runs YoloV7 - object detection node.
+```
+roslaunch yolov7_ros yolov7.launch
+```
+### Add Collision Object
+This executables generates XYZ values of detected object and adds in planning scene.
+
+This executable gets XY coordinates of detected object from `/yolov7/yolov7` topic and generates XYZ value of that object according to camera. These values are published to `/coordinates_of_object` topic.
+```
+rosrun air_moveit_config detect_and_add
+```
+This executable gets the coordinates of object from `/coordinates_of_object` topic and adds to planning scene.
+```
+rosrun air_moveit_config add_sphere
 ```
 
-## YoloV7 Object Detection
-Works real time on CPU 
+## MoveIt Task Constructor
 ```
-rosrun air_moveit_config detect.py
+roslaunch air_moveit_config air_mtc.launch
 ```
diff --git a/air_description/urdf/air.urdf b/air_description/urdf/air.urdf
@@ -59,7 +59,7 @@
     name="virtual_camera_joint"
     type="fixed">
     <origin
-      xyz="0 0 1.3"
+      xyz="0 0 1.2"
       rpy="0 0.5 0" />
     <parent
       link="base_link"/>
@@ -464,7 +464,7 @@
   <joint name="yawj" type="revolute">
     <origin rpy="0 -1.57079632679 3.14159265359" xyz="0.0464 0 0.01 "/>
     <parent link="pitch"/>
-    <child link="qbhand_base_link"/>
+    <child link="root_link"/>
     <axis
       xyz="0 0 1" />
     <limit
@@ -473,6 +473,17 @@
       effort="0"
       velocity="1.75" />
   </joint>
+
+
+  <link name="root_link"/>
+
+  <joint name="virtual_root_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0 "/>
+    <parent link="root_link"/>
+    <child link="qbhand_base_link"/>
+  </joint>
+
+
   <link name="qbhand_base_link">
     <inertial>
       <mass value="0.45"/>
@@ -551,7 +562,8 @@
     <axis xyz="0 0 -1"/>
     <dynamics damping="0.0" friction="0.0"/>
     <limit effort="100" lower="0.0" upper="1.571" velocity="100"/>
-    <mimic joint="qbhand_synergy_joint" multiplier="2.542"/>
+    <mimic joint="qbhand_synergy_joint" multiplier="1"/>
+    <!--mimic joint="qbhand_synergy_joint" multiplier="2.542"/-->
   </joint>
   <transmission name="qbhand_transmission">
     <type>transmission_interface/qbMoveTransmission</type>

diff --git a/air_hardware_interface/CMakeLists.txt b/air_hardware_interface/CMakeLists.txt
@@ -52,3 +52,7 @@ add_executable(air_right_arm_hw_interface_node
 
 target_link_libraries(air_right_arm_hw_interface_node ${catkin_LIBRARIES} ADSLIB)
 target_link_directories(air_right_arm_hw_interface_node PUBLIC ${AdsLib_INCLUDE_DIRS})
+
+install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
diff --git a/air_hardware_interface/include/air_right_arm_interface.hpp b/air_hardware_interface/include/air_right_arm_interface.hpp
@@ -29,7 +29,7 @@ namespace air
         static double toDegree(double radian);
 
         double elbowRadianToLinearPosition(double position_in_radian);
-        double elbowRadianToLinearVelocity(double position_in_radian, double velocity_in_radian);
+        double elbowRadianToLinearVelocity(double velocity_in_radian);
         double elbowLinearToRadianPosition(double position_in_mm);
         double elbowLinearToRadianVelocity(double velocity_in_mm);
 
@@ -74,13 +74,17 @@ namespace air
         std::vector<std::string> m_JointNames;
         std::vector<double> m_JointPositions;
 
-        AmsNetId m_RemoteNetID {192, 168, 6, 29, 1, 1};
+        AmsNetId m_RemoteNetID  { 192, 168, 56, 101, 1, 1 }; //TcBSD
 
-        const std::string m_RemoteIpV4 = "192.168.6.29";
+                //{172, 18, 212, 67, 1, 1}; {41, 118, 185 , 65 ,1 ,1}; {192, 168, 6, 29, 1, 1};
+
+        const std::string m_RemoteIpV4 =  "192.168.56.101"; //TcBSD
+
+                //"169.254.186.54"; "192.168.225.129"; "192.168.6.29";
 
         AdsDevice* m_Route;
 
     };
 }
 
-#endif
+#endif
diff --git a/air_hardware_interface/src/air_arm_hardware_interface.cpp b/air_hardware_interface/src/air_arm_hardware_interface.cpp
@@ -10,10 +10,14 @@ namespace air
 
         m_ControllerManager.reset(new controller_manager::ControllerManager(this, m_NodeHandle));
 
-        ros::Duration updateFrequency = ros::Duration(1.0 / m_LoopFrequency);
-
+
         m_NodeHandle.param("/arm/hardware_interface/loop_hz", m_LoopFrequency, 0.1);
 
+        ros::Duration updateFrequency = ros::Duration(1.0 / m_LoopFrequency);
+        //m_NonRealtimeLoop = m_NodeHandle.createTimer(updateFrequency, &RightArmHardwareInterface::update, this);
+
+
+
         m_NonRealtimeLoop = m_NodeHandle.createTimer(updateFrequency, &RightArmHardwareInterface::update, this);
 
     }
@@ -129,4 +133,4 @@ int main(int argc, char** argv)
 
     return 0;      
 
-}
+}