and joint_controller which support controller based on trajectory and…

… position

CMakeLists.txt

@@ -54,6 +54,9 @@ target_link_libraries(joint_position_controller ${PROJECT_NAME})
 add_executable(joint_trajectory_controller nodes/joint_trajectory_controller_node.cpp)
 target_link_libraries(joint_trajectory_controller ${PROJECT_NAME})
 
+add_executable(joint_controller nodes/joint_controller_node.cpp)
+target_link_libraries(joint_controller ${PROJECT_NAME})
+
 # for ignition-gazebo plugin RobotiqController
 add_library(RobotiqController SHARED plugins/robotiq_controller/RobotiqController.cpp)
 
@@ -82,7 +85,7 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 # Install executables
-install(TARGETS joint_state_publisher joint_position_controller joint_trajectory_controller 
+install(TARGETS joint_state_publisher joint_position_controller joint_trajectory_controller joint_controller
   DESTINATION lib/${PROJECT_NAME} 
 )
 

launch/ur10_ign.launch.py

@@ -28,9 +28,9 @@ def generate_launch_description():
     # parameter for ur10 controller
     joint_names_list=["shoulder_pan_joint","shoulder_lift_joint","elbow_joint",
                     "wrist_1_joint","wrist_2_joint","wrist_3_joint"]
-    ign_cmd_joint_topics_list=[]
+    ign_joint_topics_list=[]
     for joint_name in joint_names_list:
-        ign_cmd_joint_topics_list.append("/model/ur10/joint/%s/0/cmd_pos"%joint_name)
+        ign_joint_topics_list.append("/model/ur10/joint/%s/0/cmd_pos"%joint_name)
 
     # ros<-ign, joint state publisher for ur10
     joint_state_publisher=Node(package='universal_robot_ign', 
@@ -42,15 +42,15 @@ def generate_launch_description():
                         ],
                 output='screen') 
     #  ros->ign,  joint controller for ur10
-    joint_trajectory_controller=Node(package='universal_robot_ign', 
-                executable='joint_trajectory_controller',
-                name="ur10_joint_trajectory_controller",
+    joint_controller=Node(package='universal_robot_ign', 
+                executable='joint_controller',
+                name="ur10_joint_controller",
                 parameters=[{"joint_names": joint_names_list},
-                            {"ign_cmd_joint_topics": ign_cmd_joint_topics_list},
+                            {"ign_joint_topics": ign_joint_topics_list},
                             {"rate":200},
                            ],
                 output='screen') 
 
     return LaunchDescription([
-        ignition_simulator,joint_state_publisher,joint_trajectory_controller
+        ignition_simulator,joint_state_publisher,joint_controller
     ])

launch/ur10_robotiq140_demo.launch.py

@@ -19,7 +19,7 @@ def generate_launch_description():
             executable='joint_state_publisher_gui',
             name="joint_state_publisher_gui",
             arguments=[ urdf ],
-            remappings=[('/joint_states', '/cmd_joint_states')],
+            remappings=[('/joint_states', '/set_joint_states')],
             output='screen') 
 
     return LaunchDescription([

launch/ur10_robotiq140_ign.launch.py

@@ -28,9 +28,9 @@ def generate_launch_description():
     # parameter for ur10 controller
     joint_names_list=["shoulder_pan_joint","shoulder_lift_joint","elbow_joint",
                     "wrist_1_joint","wrist_2_joint","wrist_3_joint"]
-    ign_cmd_joint_topics_list=[]
+    ign_joint_topics_list=[]
     for joint_name in joint_names_list:
-        ign_cmd_joint_topics_list.append("/model/ur10/joint/%s/0/cmd_pos"%joint_name)
+        ign_joint_topics_list.append("/model/ur10/joint/%s/0/cmd_pos"%joint_name)
 
     # ros<-ign, joint state publisher for ur10
     joint_state_publisher=Node(package='universal_robot_ign', 
@@ -42,11 +42,12 @@ def generate_launch_description():
                         ],
                 output='screen') 
     #  ros->ign,  joint controller for ur10
-    joint_position_controller=Node(package='universal_robot_ign', 
-                executable='joint_position_controller',
-                name="ur10_joint_position_controller",
+    joint_controller=Node(package='universal_robot_ign', 
+                executable='joint_controller',
+                name="ur10_joint_controller",
                 parameters=[{"joint_names": joint_names_list},
-                            {"ign_cmd_joint_topics": ign_cmd_joint_topics_list},
+                            {"ign_joint_topics": ign_joint_topics_list},
+                            {"rate":200},
                            ],
                 output='screen') 
     # ros->ign, ign bridge to control Robotiq140 Gripper
@@ -59,5 +60,5 @@ def generate_launch_description():
             output='screen'
     )
     return LaunchDescription([
-        ignition_simulator,joint_state_publisher,joint_position_controller,ros_ign_bridge
+        ignition_simulator,joint_state_publisher,joint_controller,ros_ign_bridge
     ])

nodes/joint_controller_node.cpp

@@ -0,0 +1,42 @@
+/*******************************************************************************
+ *  Copyright (c) Gezp (https://github.com/gezp), All rights reserved.
+ *
+ *  This program is free software: you can redistribute it and/or modify it 
+ *  under the terms of the MIT License, See the MIT License for more details.
+ *
+ *  You should have received a copy of the MIT License along with this program.
+ *  If not, see <https://opensource.org/licenses/MIT/>.
+ *
+ ******************************************************************************/
+#include <rclcpp/rclcpp.hpp>
+#include <universal_robot_ign/joint_position_controller.hpp>
+#include <universal_robot_ign/joint_trajectory_controller.hpp>
+
+int main(int argc, char* argv[])
+{
+    //creat ros2 node
+    rclcpp::init(argc, argv);
+    auto ros_node = std::make_shared<rclcpp::Node>("joint_controller");
+    // variable
+    std::vector<std::string> joint_names;
+    std::vector<std::string> ign_joint_topics;
+    int update_rate;
+    // parameters
+    ros_node->declare_parameter("joint_names");
+    ros_node->declare_parameter("ign_joint_topics");
+    ros_node->declare_parameter("rate", 200);
+    joint_names = ros_node->get_parameter("joint_names").as_string_array();
+    ign_joint_topics = ros_node->get_parameter("ign_joint_topics").as_string_array();
+    update_rate = ros_node->get_parameter("rate").as_int();
+    // create controller
+    auto joint_trajectory_controller = std::make_shared<universal_robot_ign::jointTrajectoryController>(ros_node,
+        joint_names, "set_joint_trajectory", ign_joint_topics ,update_rate);
+    // create controller 
+    auto joint_position_controller = std::make_shared<universal_robot_ign::JointPositionController>(ros_node,
+        joint_names, "set_joint_states", ign_joint_topics);
+    // run node until it's exited
+    rclcpp::spin(ros_node);
+    //clean up
+    rclcpp::shutdown();
+    return 0;
+}

nodes/joint_position_controller_node.cpp

@@ -19,17 +19,15 @@ int main(int argc, char* argv[])
     // variables
     std::vector<std::string> joint_names;
     std::string cmd_joint_states_topic;
-    std::vector<std::string> ign_cmd_joint_topics;
+    std::vector<std::string> ign_joint_topics;
     // parameters
     ros_node->declare_parameter("joint_names");
-    ros_node->declare_parameter("cmd_joint_states_topic","cmd_joint_states");
-    ros_node->declare_parameter("ign_cmd_joint_topics");
+    ros_node->declare_parameter("ign_joint_topics");
     joint_names = ros_node->get_parameter("joint_names").as_string_array();
-    cmd_joint_states_topic = ros_node->get_parameter("cmd_joint_states_topic").as_string();
-    ign_cmd_joint_topics = ros_node->get_parameter("ign_cmd_joint_topics").as_string_array();
+    ign_joint_topics = ros_node->get_parameter("ign_joint_topics").as_string_array();
     // create controller 
     auto joint_controller = std::make_shared<universal_robot_ign::JointPositionController>(ros_node,
-        joint_names, cmd_joint_states_topic, ign_cmd_joint_topics);
+        joint_names, "set_joint_states", ign_joint_topics);
     // run node until it's exited
     rclcpp::spin(ros_node);
     //clean up

nodes/joint_state_publisher_node.cpp

@@ -26,10 +26,8 @@ int main(int argc, char* argv[])
     ros_node->declare_parameter("joint_names");
     ros_node->declare_parameter("ign_joint_states_topic");
     ros_node->declare_parameter("ign_joint_idxs");
-    ros_node->declare_parameter("joint_states_topic", "joint_states");
     ros_node->declare_parameter("rate", 30);
     joint_names = ros_node->get_parameter("joint_names").as_string_array();
-    joint_states_topic = ros_node->get_parameter("joint_states_topic").as_string();
     ign_joint_states_topic = ros_node->get_parameter("ign_joint_states_topic").as_string();
     std::vector<int64_t> ign_joint_idxs_64 = ros_node->get_parameter("ign_joint_idxs").as_integer_array();
     update_rate = ros_node->get_parameter("rate").as_int();
@@ -44,7 +42,7 @@ int main(int argc, char* argv[])
     }
     // create publisher
     auto joint_publisher = std::make_shared<universal_robot_ign::JointStatePublisher>(ros_node,
-        joint_names, joint_states_topic, ign_joint_states_topic, ign_joint_idxs ,update_rate);
+        joint_names, "joint_states", ign_joint_states_topic, ign_joint_idxs ,update_rate);
     // run node until it's exited
     rclcpp::spin(ros_node);
     //clean up

nodes/joint_trajectory_controller_node.cpp

@@ -18,21 +18,19 @@ int main(int argc, char* argv[])
     auto ros_node = std::make_shared<rclcpp::Node>("joint_trajectory_controller");
     // variable
     std::vector<std::string> joint_names;
-    std::vector<std::string> ign_cmd_joint_topics;
+    std::vector<std::string> ign_joint_topics;
     std::string cmd_joint_trajectory_topic;
     int update_rate;
     // parameters
     ros_node->declare_parameter("joint_names");
-    ros_node->declare_parameter("cmd_joint_trajectory_topic","cmd_joint_trajectory");
-    ros_node->declare_parameter("ign_cmd_joint_topics");
+    ros_node->declare_parameter("ign_joint_topics");
     ros_node->declare_parameter("rate", 200);
     joint_names = ros_node->get_parameter("joint_names").as_string_array();
-    cmd_joint_trajectory_topic = ros_node->get_parameter("cmd_joint_trajectory_topic").as_string();
-    ign_cmd_joint_topics = ros_node->get_parameter("ign_cmd_joint_topics").as_string_array();
+    ign_joint_topics = ros_node->get_parameter("ign_joint_topics").as_string_array();
     update_rate = ros_node->get_parameter("rate").as_int();
     // create controller
     auto joint_controller = std::make_shared<universal_robot_ign::jointTrajectoryController>(ros_node,
-        joint_names, cmd_joint_trajectory_topic, ign_cmd_joint_topics ,update_rate);
+        joint_names, "set_joint_trajectory", ign_joint_topics ,update_rate);
     // run node until it's exited
     rclcpp::spin(ros_node);
     //clean up

plugins/robotiq_controller/RobotiqController.cpp

@@ -71,7 +71,7 @@ RobotiqController::RobotiqController() : dataPtr(std::make_unique<RobotiqControl
 }
 
 void RobotiqController::Configure(const Entity &_entity,
-                                  const std::shared_ptr<const sdf::Element> &_sdf,
+                                  const std::shared_ptr<const sdf::Element> &/*_sdf*/,
                                   EntityComponentManager &_ecm,
                                   EventManager & /*_eventMgr*/) {
     this->dataPtr->model = Model(_entity);

scripts/moveit2.py

@@ -65,7 +65,7 @@ def init_robot(self):
 
         # Publisher of trajectories
         self.joint_trajectory_pub = self.create_publisher(JointTrajectory,
-                                                          "cmd_joint_trajectory", 1)
+                                                          "set_joint_trajectory", 1)
 
         # Subscriber of current joint states
         self.joint_state = JointState()