Merge pull request AuboRobot#72 from laughing920/noetic

feat:support of Aubo Robot ROS-Noetic
APPLz95 · Mar 22, 2022 · 98d832a · 98d832a
2 parents 10f74e4 + d3cf56b
commit 98d832a
Show file tree

Hide file tree

Showing 149 changed files with 2,097,095 additions and 124 deletions.
diff --git a/README.md b/README.md
@@ -1,12 +1,12 @@
 # AUBO Robot
-**Maintainer: zhaoyu@aubo-robotics.cn**
+**Maintainer: chenshudong@aubo-robotics.cn**
 
 
 
 
 * Aubo_robot [ROS](http://www.ros.org/) meta-package for [ROS-Industrial](http://wiki.ros.org/Industrial). See the [ROS wiki](http://wiki.ros.org/)  page for more information.
 
-* This repository provides ROS support for [AUBO robots](https://aubo-robotics.com/en/). This repository holds source code for `kinetic`. The corresponding robot controller software version is `V4`. For those software version < `V4`, can refer to [here]( http://wiki.ros.org/aubo_robot).
+* This repository provides ROS support for [AUBO robots](https://aubo-robotics.com/en/). This repository holds source code for 'Noetic'. The corresponding robot controller software version is 'V4'. For those software version < 'V4', can refer to [here]( http://wiki.ros.org/aubo_robot).
 
 ### Installation from Source
 ---

diff --git a/aubo_controller/CMakeLists.txt b/aubo_controller/CMakeLists.txt
@@ -57,4 +57,4 @@ install(TARGETS aubo_joint_trajectory_action DESTINATION ${CATKIN_PACKAGE_BIN_DE
 #install(TARGETS aubo_simulator_controller DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 catkin_install_python(PROGRAMS script/aubo_controller/aubo_robot_simulator
-   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
diff --git a/aubo_controller/include/joint_trajectory_action.h b/aubo_controller/include/joint_trajectory_action.h
@@ -192,7 +192,7 @@ class JointTrajectoryAction
    * \param gh goal handle
    *
    */
-  void goalCB(JointTractoryActionServer::GoalHandle & gh);
+  void goalCB(const JointTractoryActionServer::GoalHandle &gh);
 
   /**
    * \brief Action server cancel callback method
@@ -201,7 +201,7 @@ class JointTrajectoryAction
    *
    */
 
-  void cancelCB(JointTractoryActionServer::GoalHandle & gh);
+  void cancelCB(const JointTractoryActionServer::GoalHandle &gh);
   /**
    * \brief Controller state callback (executed when feedback message
    * received)

diff --git a/aubo_controller/script/aubo_controller/aubo_robot_simulator b/aubo_controller/script/aubo_controller/aubo_robot_simulator
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 #
 # Software License Agreement (BSD License)
 # Copyright (c) 2017-2018, Aubo Robotics
@@ -31,7 +31,15 @@
 import rospy
 import copy
 import threading
-import Queue
+import sys
+import os
+path = os.path.dirname(os.path.realpath(__file__))
+sys.path.insert(0,path)
+print(path)
+if sys.version > '3':
+    import queue as Queue
+else:
+    import Queue
 
 # Publish
 from sensor_msgs.msg import JointState
@@ -129,6 +137,10 @@ class MotionControllerSimulator():
     # The function will recieve info about cancel trajectory because user want the robot stop when the robot running
     def Is_cancel_trajectory_callback(self, msg_in):
         self.cancle_trajectory = msg_in.data
+        if self.cancle_trajectory == 1:
+            if not self.motion_buffer.empty():
+                self._clear_buffer()
+            self.cancle_trajectory = 0
         rospy.loginfo('is cancel trajectory status %d' , self.cancle_trajectory)
 
 
@@ -190,7 +202,7 @@ class MotionControllerSimulator():
     """
     def _move_to(self, point, dur):
         while self.ribBufferSize > self.MinimumBufferSize:
-            rospy.loginfo('The number of driver Buffer Data is big enough, not send this time: %d', self.ribBufferSize)
+            #rospy.loginfo('The number of driver Buffer Data is big enough, not send this time: %d', self.ribBufferSize)
             rospy.sleep(dur)
 
         if self.ribBufferSize == 0 and self.ControllerConnectedFlag == 0:      # motion start or no robot connected!
@@ -221,7 +233,7 @@ class MotionControllerSimulator():
 
         rospy.loginfo('Starting motion worker in motion controller simulator')
         move_duration = rospy.Duration()
-        if self.update_rate <> 0.:
+        if self.update_rate != 0:
             update_duration = rospy.Duration(1./self.update_rate)
         last_goal_point = JointTrajectoryPoint()
 
@@ -253,6 +265,7 @@ class MotionControllerSimulator():
                     a3 = [0] *N
                     a4 = [0] *N
                     a5 = [0] *N
+
                     for i in range(0,N):
                         a2[i] = 0.5 * last_goal_point.accelerations[i]
                         h[i] = current_goal_point.positions[i] - last_goal_point.positions[i]
@@ -262,6 +275,7 @@ class MotionControllerSimulator():
                     if self.update_rate > 0:
                         tt = last_goal_point.time_from_start.to_sec()
                         ti = tt
+
                         while ((tt < current_goal_point.time_from_start.to_sec()) and (0 == self.cancle_trajectory)):
                             t1 = tt - ti
                             t2 = t1 * t1
@@ -283,14 +297,16 @@ class MotionControllerSimulator():
                 self.joint_state_publisher()
                 last_goal_point = copy.deepcopy(current_goal_point)
 
+
             except Exception as e:
                 rospy.logerr('Unexpected exception: %s', e)
 
             # if user want stop ,we will clear Trajectory buffer data from moveit
-            if self.cancle_trajectory == 1:
-                self.cancle_trajectory = 0
-                while not self.motion_buffer.empty():
-                    self.motion_buffer.get()
+            #if self.cancle_trajectory == 1:
+            #    if not self.motion_buffer.empty():
+            #        self.motion_buffer.queue.clear()
+            #    self.cancle_trajectory = 0
+            #    rospy.logerr("ssssss")
 
         rospy.logdebug("Shutting down motion controller")
 
@@ -392,7 +408,7 @@ class AuboRobotSimulatorNode():
     feedback state (as these are closely related)
     """
     def joint_state_publisher(self):
-        if self.EnableFlag == 1 and self.motion_ctrl.positionUpdatedFlag == '1':
+        if 1:#self.EnableFlag == 1 and self.motion_ctrl.positionUpdatedFlag == '1':
             try:
                 joint_state_msg = JointState()
                 joint_fb_msg = FollowJointTrajectoryFeedback()
@@ -410,7 +426,7 @@ class AuboRobotSimulatorNode():
                     joint_fb_msg.joint_names = self.joint_names
                     joint_fb_msg.actual.positions = self.motion_ctrl.get_joint_positions()
 
-                    # self.joint_feedback_pub.publish(joint_fb_msg)
+                    self.joint_feedback_pub.publish(joint_fb_msg)
 
             except Exception as e:
                 rospy.loginfo('Unexpected exception in joint state publisher: %s', e)
@@ -456,6 +472,9 @@ class AuboRobotSimulatorNode():
             rospy.logdebug('handle joint_path_command')
             try:
                 rospy.loginfo('Received trajectory with %s points, executing callback', str(len(msg_in.points)))
+                #if self.motion_ctrl.cancle_trajectory:
+                #    while self.motion_ctrl.cancle_trajectory:
+                #        rospy.spin()
 
                 if self.motion_ctrl.is_in_motion():
                     if len(msg_in.points) > 0:
@@ -466,14 +485,15 @@ class AuboRobotSimulatorNode():
                     self.motion_ctrl.stop()
 
                 else:
+
                     self.velocity_scale_factor = rospy.get_param('/aubo_controller/velocity_scale_factor', 1.0)
                     rospy.loginfo('The velocity scale factor is: %s', str(self.velocity_scale_factor))
                     new_traj = scale_trajectory_speed(msg_in, self.velocity_scale_factor)
                     for point in new_traj.points:
                         # first remaps point to controller joint order, the add the point to the controller.
                         point = self._to_controller_order(msg_in.joint_names, point)
                         self.motion_ctrl.add_motion_waypoint(point)
-#                        rospy.loginfo('Add new position: %s', str(point.positions))
+                    rospy.loginfo('Add new position: %s', str(point.positions))
 
             except Exception as e:
                 rospy.logerr('Unexpected exception: %s', e)

diff --git a/aubo_controller/script/aubo_controller/trajectory_speed.py b/aubo_controller/script/aubo_controller/trajectory_speed.py
@@ -1,4 +1,4 @@
-#!/usr/bin/env python
+#!/usr/bin/env python3
 
 # Software License Agreement (BSD License)
 # Copyright (c) 2017-2018, Aubo Robotics
@@ -147,4 +147,4 @@ def create_tracking_trajectory(traj, speed, min_speed):
         new_traj.joint_trajectory.points[i].accelerations = [speed / 4.0] * n_joints
 
     # Return the new trajecotry
-    return new_traj
+    return new_traj
diff --git a/aubo_controller/src/joint_trajectory_action.cpp b/aubo_controller/src/joint_trajectory_action.cpp
@@ -137,25 +137,26 @@ void JointTrajectoryAction::watchdog(const ros::TimerEvent &e)
   }
 }
 
-void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
+void JointTrajectoryAction::goalCB(const JointTractoryActionServer::GoalHandle &gh)
 {
   ROS_INFO("Received new goal");
+  JointTractoryActionServer::GoalHandle tmp_gh = const_cast<JointTractoryActionServer::GoalHandle&>(gh);
 
   // reject all goals as long as we haven't heard from the remote controller
   if (!controller_alive_)
   {
     ROS_ERROR("Joint trajectory action rejected: waiting for (initial) feedback from controller");
     control_msgs::FollowJointTrajectoryResult rslt;
     rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
-    gh.setRejected(rslt, "Waiting for (initial) feedback from controller");
+    tmp_gh.setRejected(rslt, "Waiting for (initial) feedback from controller");
 
     // no point in continuing: already rejected
     return;
   }
 
-  if (!gh.getGoal()->trajectory.points.empty())
+  if (!tmp_gh.getGoal()->trajectory.points.empty())
   {
-    if (industrial_utils::isSimilar(joint_names_, gh.getGoal()->trajectory.joint_names))
+    if (industrial_utils::isSimilar(joint_names_, tmp_gh.getGoal()->trajectory.joint_names))
     {
 
       // Cancels the currently active goal.
@@ -165,8 +166,8 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
         abortGoal();
       }
 
-      gh.setAccepted();
-      active_goal_ = gh;
+      tmp_gh.setAccepted();
+      active_goal_ = tmp_gh;
       has_active_goal_ = true;
       time_to_check_ = ros::Time::now() +
           ros::Duration(active_goal_.getGoal()->trajectory.points.back().time_from_start.toSec() / 2.0);
@@ -183,36 +184,36 @@ void JointTrajectoryAction::goalCB(JointTractoryActionServer::GoalHandle & gh)
       ROS_ERROR("Joint trajectory action failing on invalid joints");
       control_msgs::FollowJointTrajectoryResult rslt;
       rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_JOINTS;
-      gh.setRejected(rslt, "Joint names do not match");
+      tmp_gh.setRejected(rslt, "Joint names do not match");
     }
   }
   else
   {
     ROS_ERROR("Joint trajectory action failed on empty trajectory");
     control_msgs::FollowJointTrajectoryResult rslt;
     rslt.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL;
-    gh.setRejected(rslt, "Empty trajectory");
+    tmp_gh.setRejected(rslt, "Empty trajectory");
   }
 
   // Adding some informational log messages to indicate unsupported goal constraints
-  if (gh.getGoal()->goal_time_tolerance.toSec() > 0.0)
+  if (tmp_gh.getGoal()->goal_time_tolerance.toSec() > 0.0)
   {
     ROS_WARN_STREAM("Ignoring goal time tolerance in action goal, may be supported in the future");
   }
-  if (!gh.getGoal()->goal_tolerance.empty())
+  if (!tmp_gh.getGoal()->goal_tolerance.empty())
   {
     ROS_WARN_STREAM(
         "Ignoring goal tolerance in action, using paramater tolerance of " << goal_threshold_ << " instead");
   }
-  if (!gh.getGoal()->path_tolerance.empty())
+  if (!tmp_gh.getGoal()->path_tolerance.empty())
   {
     ROS_WARN_STREAM("Ignoring goal path tolerance, option not supported by ROS-Industrial drivers");
   }
 }
 
-void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle & gh)
+void JointTrajectoryAction::cancelCB(const JointTractoryActionServer::GoalHandle &gh)
 {
-
+  JointTractoryActionServer::GoalHandle tmp_gh = const_cast<JointTractoryActionServer::GoalHandle&>(gh);
   ROS_DEBUG("Received action cancel request");
   if (active_goal_ == gh)
   {
@@ -222,7 +223,7 @@ void JointTrajectoryAction::cancelCB(JointTractoryActionServer::GoalHandle & gh)
     pub_trajectory_command_.publish(empty);
 
     // Marks the current goal as canceled.
-    active_goal_.setCanceled();
+    tmp_gh.setCanceled();
     has_active_goal_ = false;
   }
   else
@@ -495,6 +496,10 @@ int main(int argc, char** argv)
           controller_name = "aubo_i5l_controller/follow_joint_trajectory";
   else if(robot_name == "aubo_i16")
           controller_name = "aubo_i16_controller/follow_joint_trajectory";
+  else if(robot_name == "aubo_e5")
+          controller_name = "aubo_e5_controller/follow_joint_trajectory";
+  else if(robot_name == "aubo_e3")
+          controller_name = "aubo_e3_controller/follow_joint_trajectory";
 
   JointTrajectoryAction action(controller_name);
   action.run();

diff --git a/aubo_demo/CMakeLists.txt b/aubo_demo/CMakeLists.txt
@@ -4,7 +4,49 @@ project(aubo_demo)
 ## Compile as C++11, supported in ROS Kinetic and newer
 add_compile_options(-std=c++11)
 
+if("$ENV{ROS_DISTRO}" STREQUAL "noetic")
+    find_package(catkin REQUIRED COMPONENTS
+      roscpp
+      rospy
+      std_msgs
+      moveit_core
+      moveit_visual_tools
+      moveit_ros_planning
+      moveit_ros_planning_interface
+      pluginlib
+      geometric_shapes
+      pcl_ros
+      pcl_conversions
+      rosbag
+      tf
+      tf2_ros
+      #tf2_eigen
+      tf2_geometry_msgs
+    )
+    Message(STATUS "<<<<<<<<<<<<<<<<<<<<<<<<<Note: Find Package for noetic")
+endif()
 
+if("$ENV{ROS_DISTRO}" STREQUAL "melodic")
+    find_package(catkin REQUIRED COMPONENTS
+      roscpp
+      rospy
+      std_msgs
+      moveit_core
+      moveit_visual_tools
+      moveit_ros_planning
+      moveit_ros_planning_interface
+      pluginlib
+      geometric_shapes
+      pcl_ros
+      pcl_conversions
+      rosbag
+      tf
+      tf2_ros
+      #tf2_eigen
+      tf2_geometry_msgs
+    )
+    Message(STATUS "<<<<<<<<<<<<<<<<<<<<<<<<<Note: Find Package for melodic")
+endif()
 
 if("$ENV{ROS_DISTRO}" STREQUAL "kinetic")
     find_package(catkin REQUIRED COMPONENTS
@@ -73,7 +115,27 @@ include_directories(
   ${catkin_INCLUDE_DIRS}
 )
 
+if("$ENV{ROS_DISTRO}" STREQUAL "noetic")
+    ## Declare a C++ executable
+    ## With catkin_make MoveGroupInterface_To_Kinetic package is built within a single CMake context
+    ## The recommended prefix ensures that target names across packages don't collide
+    add_executable(MoveGroupInterface_To_Noetic src/MoveGroupInterface_To_Noetic.cpp)
+
+    ## Specify libraries to link a library or executable target against
+    target_link_libraries(MoveGroupInterface_To_Noetic ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+    Message(STATUS "<<<<<<<<<<<<<<<<<<<<<<<<<Note: build aubo_demo node for Noetic")
+endif()
 
+if("$ENV{ROS_DISTRO}" STREQUAL "melodic")
+    ## Declare a C++ executable
+    ## With catkin_make MoveGroupInterface_To_Kinetic package is built within a single CMake context
+    ## The recommended prefix ensures that target names across packages don't collide
+    add_executable(MoveGroupInterface_To_Melodic src/MoveGroupInterface_To_Melodic.cpp)
+
+    ## Specify libraries to link a library or executable target against
+    target_link_libraries(MoveGroupInterface_To_Melodic ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+    Message(STATUS "<<<<<<<<<<<<<<<<<<<<<<<<<Note: build aubo_demo node for Melodic")
+endif()
 
 if("$ENV{ROS_DISTRO}" STREQUAL "kinetic")
     ## Declare a C++ executable

diff --git a/aubo_demo/launch/MoveGroupInterface_To_Melodic.launch b/aubo_demo/launch/MoveGroupInterface_To_Melodic.launch
@@ -0,0 +1,6 @@
+<launch>
+
+  <node name="MoveGroupInterface_To_Melodic" pkg="aubo_demo" type="MoveGroupInterface_To_Melodic" respawn="false" output="screen">
+  </node>
+
+</launch>
diff --git a/aubo_demo/launch/MoveGroupInterface_To_Noetic.launch b/aubo_demo/launch/MoveGroupInterface_To_Noetic.launch
@@ -0,0 +1,6 @@
+<launch>
+
+  <node name="MoveGroupInterface_To_Noetic" pkg="aubo_demo" type="MoveGroupInterface_To_Noetic" respawn="false" output="screen">
+  </node>
+
+</launch>