From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 2 Dec 2024 04:19:11 +0000 (-0300)
Subject: Change q2d_teleop to use MultiDOF PID controller.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=2b60404539f1b153f639ff60500aee353ffacc08;p=q2d.git

Change q2d_teleop to use MultiDOF PID controller.
---

diff --git a/q2d_description/config/gz_bridge.yaml b/q2d_description/config/ros_gz_bridge.yaml
similarity index 100%
rename from q2d_description/config/gz_bridge.yaml
rename to q2d_description/config/ros_gz_bridge.yaml
diff --git a/q2d_description/launch/gazebo.launch.xml b/q2d_description/launch/gazebo.launch.xml
index 0260838..45b06fd 100644
--- a/q2d_description/launch/gazebo.launch.xml
+++ b/q2d_description/launch/gazebo.launch.xml
@@ -42,7 +42,7 @@
 	<node name="q2d_spawner" pkg="ros_gz_sim" exec="create" args="-topic robot_description -name q2d"/>
 
 	<node name="ros_gz_bridge" pkg="ros_gz_bridge" exec="parameter_bridge">
-		<param name="config_file" value="$(find-pkg-share q2d_description)/config/gz_bridge.yaml"/>
+		<param name="config_file" value="$(find-pkg-share q2d_description)/config/ros_gz_bridge.yaml"/>
 		<param name="use_sim_time" value="$(var use_sim_time)"/>
 	</node>
 </launch>
diff --git a/q2d_teleop/CMakeLists.txt b/q2d_teleop/CMakeLists.txt
index 8e774b1..fe392b8 100644
--- a/q2d_teleop/CMakeLists.txt
+++ b/q2d_teleop/CMakeLists.txt
@@ -14,25 +14,27 @@ find_package(kdl_parser REQUIRED)
 find_package(orocos_kdl REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
 find_package(urdf REQUIRED)
 
-add_executable(q2d_teleop_tablet src/q2d_teleop_tablet.cpp)
-target_include_directories(q2d_teleop_tablet PUBLIC
+add_executable(q2d_teleop_joy src/q2d_teleop_joy.cpp)
+target_include_directories(q2d_teleop_joy PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
-target_compile_features(q2d_teleop_tablet PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_compile_features(q2d_teleop_joy PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 ament_target_dependencies(
-  q2d_teleop_tablet
+  q2d_teleop_joy
   "rclcpp"
   "geometry_msgs"
   "kdl_parser"
   "orocos_kdl"
   "sensor_msgs"
   "std_msgs"
+  "control_msgs"
   "urdf"
 )
 
-#install(TARGETS q2d_teleop_tablet
+#install(TARGETS q2d_teleop_joy
 #  DESTINATION lib/${PROJECT_NAME})
 
 add_executable(q2d_teleop_rviz src/q2d_teleop_rviz.cpp)
@@ -48,10 +50,11 @@ ament_target_dependencies(
   "orocos_kdl"
   "sensor_msgs"
   "std_msgs"
+  "control_msgs"
   "urdf"
 )
 
-install(TARGETS q2d_teleop_tablet q2d_teleop_rviz
+install(TARGETS q2d_teleop_joy q2d_teleop_rviz
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch config
diff --git a/q2d_teleop/config/display.rviz b/q2d_teleop/config/display.rviz
index 1cfaa99..9afafd9 100644
--- a/q2d_teleop/config/display.rviz
+++ b/q2d_teleop/config/display.rviz
@@ -83,6 +83,10 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        World:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         base_link:
           Alpha: 1
           Show Axes: false
@@ -116,6 +120,9 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+      Mass Properties:
+        Inertia: false
+        Mass: false
       Name: RobotModel
       TF Prefix: ""
       Update Interval: 0
@@ -166,26 +173,21 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 10
+      Angle: 0
+      Class: rviz_default_plugins/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
-      Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
+      Scale: 356.14495849609375
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 0.785398006439209
+      Value: TopDownOrtho (rviz_default_plugins)
+      X: 0
+      Y: 0
     Saved: ~
 Window Geometry:
   Displays:
@@ -193,7 +195,7 @@ Window Geometry:
   Height: 846
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025600fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000026300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
diff --git a/q2d_teleop/launch/display.launch.xml b/q2d_teleop/launch/display.launch.xml
index e178be0..493fe59 100644
--- a/q2d_teleop/launch/display.launch.xml
+++ b/q2d_teleop/launch/display.launch.xml
@@ -1,7 +1,7 @@
 <!--
   q2d_teleop: A ROS 2 package node to teloperate the Quanser 2DSFJE robot.
   
-  Copyright (c) 2018..2023 Walter Fetter Lages <w.fetter@ieee.org>
+  Copyright (c) 2018..2024 Walter Fetter Lages <w.fetter@ieee.org>
 
     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -24,6 +24,6 @@
 
 <launch>
 	<include file="$(find-pkg-share q2d_description)/launch/q2d.launch.xml"/>
-	<node pkg="tf2_ros" exec="static_transform_publisher" name="q2d_origin_publisher" args="--frame-id map --child-frame-id origin_link"/>
+	<node pkg="tf2_ros" exec="static_transform_publisher" name="q2d_origin_publisher" args="--frame-id map --child-frame-id World"/>
 	<node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share q2d_teleop)/config/display.rviz"/>
 </launch>
diff --git a/q2d_teleop/launch/gazebo.launch.xml b/q2d_teleop/launch/gazebo.launch.xml
index 3857a75..d5aa951 100644
--- a/q2d_teleop/launch/gazebo.launch.xml
+++ b/q2d_teleop/launch/gazebo.launch.xml
@@ -27,11 +27,9 @@
 		<arg name="pause" value="false"/>
 	</include>
 
-	<node name="q2d_teleop_tablet" pkg="q2d_teleop" exec="q2d_teleop_tablet"/>
+	<node name="q2d_teleop_joy" pkg="q2d_teleop" exec="q2d_teleop_joy"/>
 
 	<node name="q2d_teleop_rviz" pkg="q2d_teleop" exec="q2d_teleop_rviz"/>
 
 	<include file="$(find-pkg-share q2d_teleop)/launch/display.launch.xml"/>
-
-	<include file="$(find-pkg-share gfxtablet_ros)/launch/gfxtablet.launch.xml"/>
 </launch>
diff --git a/q2d_teleop/package.xml b/q2d_teleop/package.xml
index 5852db0..f162c00 100644
--- a/q2d_teleop/package.xml
+++ b/q2d_teleop/package.xml
@@ -15,6 +15,7 @@
   <depend>orocos_kdl</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
+  <depend>control_msgs</depend>
   <depend>urdf</depend>
   
   <test_depend>ament_lint_auto</test_depend>
diff --git a/q2d_teleop/src/q2d_teleop_tablet.cpp b/q2d_teleop/src/q2d_teleop_joy.cpp
similarity index 85%
rename from q2d_teleop/src/q2d_teleop_tablet.cpp
rename to q2d_teleop/src/q2d_teleop_joy.cpp
index 6b6385a..22095d7 100644
--- a/q2d_teleop/src/q2d_teleop_tablet.cpp
+++ b/q2d_teleop/src/q2d_teleop_joy.cpp
@@ -1,7 +1,7 @@
 /*
-  q2d_teleop_tablet: A ROS 2 node to teloperate the Quanser 2DSFJE robot.
+  q2d_teleop_joy: A ROS 2 node to teloperate the Quanser 2DSFJE robot.
   
-  Copyright (c) 2018, 2019, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+  Copyright (c) 2018..2024 Walter Fetter Lages <w.fetter@ieee.org>
 
     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -25,7 +25,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/logging.hpp>
 #include <sensor_msgs/msg/joy.hpp>
-#include <std_msgs/msg/float64.hpp>
+#include <control_msgs/msg/multi_dof_command.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl_parser/kdl_parser.hpp>
@@ -41,8 +41,7 @@ class Q2dTeleop: public rclcpp::Node
 			
 	private:
 	rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joySub_;
-	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr shoulderCmdPub_;
-	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr elbowCmdPub_;
+	rclcpp::Publisher<control_msgs::msg::MultiDOFCommand>::SharedPtr refPub_;
 	
 	KDL::Frame goal_;
 
@@ -55,11 +54,10 @@ class Q2dTeleop: public rclcpp::Node
 	void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription);
 };
 
-Q2dTeleop::Q2dTeleop(void): Node("q2d_teleop_tablet"), q_(2)
+Q2dTeleop::Q2dTeleop(void): Node("q2d_teleop_joy"), q_(2)
 {
 	joySub_=create_subscription<sensor_msgs::msg::Joy>("joy",100,std::bind(&Q2dTeleop::joyCB,this,std::placeholders::_1));
-	shoulderCmdPub_=create_publisher<std_msgs::msg::Float64>("shoulder_controller/command",100);
-        elbowCmdPub_=create_publisher<std_msgs::msg::Float64>("elbow_controller/command",100);
+	refPub_=create_publisher<control_msgs::msg::MultiDOFCommand>("pid_controller/reference",100);
 	
         rclcpp::QoS qos(rclcpp::KeepLast(1));
         qos.transient_local();
@@ -151,14 +149,15 @@ void Q2dTeleop::publish(void)
         }
 #endif
 
-	std_msgs::msg::Float64 shoulderCmd;
-	std_msgs::msg::Float64 elbowCmd;
-	
-	shoulderCmd.data=q_(0);
-	elbowCmd.data=q_(1);
+	control_msgs::msg::MultiDOFCommand ref;
+
+	ref.dof_names.push_back("shoulder_active_joint");
+	ref.values.push_back(q_(0));
+
+	ref.dof_names.push_back("elbow_active_joint");
+	ref.values.push_back(q_(1));
 	
-	shoulderCmdPub_->publish(shoulderCmd);
-	elbowCmdPub_->publish(elbowCmd);
+	refPub_->publish(ref);	
 }
 
 int main(int argc,char* argv[])
diff --git a/q2d_teleop/src/q2d_teleop_rviz.cpp b/q2d_teleop/src/q2d_teleop_rviz.cpp
index 88626f6..d7dde09 100644
--- a/q2d_teleop/src/q2d_teleop_rviz.cpp
+++ b/q2d_teleop/src/q2d_teleop_rviz.cpp
@@ -1,7 +1,7 @@
 /*
   q2d_teleop_rviz: A ROS 2 node to teloperate the Quanser 2DSFJE robot.
   
-  Copyright (c) 2018, 2019, 2021 Walter Fetter Lages <w.fetter@ieee.org>
+  Copyright (c) 2018..2024 Walter Fetter Lages <w.fetter@ieee.org>
 
     This program is free software; you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -26,6 +26,7 @@
 #include <geometry_msgs/msg/point_stamped.hpp>
 #include <std_msgs/msg/float64.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <control_msgs/msg/multi_dof_command.hpp>
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl_parser/kdl_parser.hpp>
 
@@ -40,8 +41,7 @@ class Q2dTeleop: public rclcpp::Node
 			
 	private:
 	rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr clickSub_;
-	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr shoulderCmdPub_;
-	rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr elbowCmdPub_;
+	rclcpp::Publisher<control_msgs::msg::MultiDOFCommand>::SharedPtr refPub_;
 	
 	KDL::Frame goal_;
 
@@ -57,8 +57,7 @@ class Q2dTeleop: public rclcpp::Node
 Q2dTeleop::Q2dTeleop(void): Node("Q2d_teleop_rviz"), q_(2)
 {
 	clickSub_=create_subscription<geometry_msgs::msg::PointStamped>("clicked_point",100,std::bind(&Q2dTeleop::clickCB,this,std::placeholders::_1));
-	shoulderCmdPub_=create_publisher<std_msgs::msg::Float64>("shoulder_controller/command",100);
-	elbowCmdPub_=create_publisher<std_msgs::msg::Float64>("elbow_controller/command",100);
+	refPub_=create_publisher<control_msgs::msg::MultiDOFCommand>("pid_controller/reference",100);
 	
 	rclcpp::QoS qos(rclcpp::KeepLast(1));
 	qos.transient_local();
@@ -148,14 +147,15 @@ void Q2dTeleop::publish(void)
 	}
 #endif
 
-	std_msgs::msg::Float64 shoulderCmd;
-	std_msgs::msg::Float64 elbowCmd;
-	
-	shoulderCmd.data=q_(0);
-	elbowCmd.data=q_(1);
+	control_msgs::msg::MultiDOFCommand ref;
+
+	ref.dof_names.push_back("shoulder_active_joint");
+	ref.values.push_back(q_(0));
+
+	ref.dof_names.push_back("elbow_active_joint");
+	ref.values.push_back(q_(1));
 	
-	shoulderCmdPub_->publish(shoulderCmd);
-	elbowCmdPub_->publish(elbowCmd);
+	refPub_->publish(ref);
 }