From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Tue, 5 Sep 2023 07:11:10 +0000 (-0300)
Subject: Fix support for two joint.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=refs%2Fheads%2Fhumble;p=q2d.git

Fix support for two joint.
---

diff --git a/q2d_bringup/launch/group_bypass.launch.xml b/q2d_bringup/launch/group_bypass.launch.xml
index 20a831f..6683992 100644
--- a/q2d_bringup/launch/group_bypass.launch.xml
+++ b/q2d_bringup/launch/group_bypass.launch.xml
@@ -23,7 +23,7 @@
 	<arg name="config" default="$(find-pkg-share q2d_bringup)/config/group_bypass.yaml"/>
 
 	<node name="group_controller_spawner" pkg="controller_manager" exec="spawner"
-		args="-t effort_controllers/JointGroupEffortController -p $(var config) group_controller"/>
+		args="-t effort_controllers/JointGroupEffortController -p $(var config) group_bypass"/>
 
 	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
 		args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
diff --git a/q2d_bringup/launch/hardware.launch.xml b/q2d_bringup/launch/hardware.launch.xml
index 38ef83c..9eccbd7 100644
--- a/q2d_bringup/launch/hardware.launch.xml
+++ b/q2d_bringup/launch/hardware.launch.xml
@@ -26,19 +26,18 @@
 	<arg name="config" default="$(find-pkg-share q2d_bringup)/config/$(var controller).yaml"/>
 
 	<include file="$(find-pkg-share q2d_hardware)/launch/controller_manager.launch.xml"/>
-<!--	
-	<include file="$(find-pkg-share q2d_bringup)/launch/$(var controller).launch.xml">
+
+	<!--include file="$(find-pkg-share q2d_bringup)/launch/$(var controller).launch.xml">
 		<arg name="config" value="$(var config)"/>
 		<arg name="use_sim_time" value="false"/>
-	</include>
--->
+	</include-->
 
 	<node name="group_controller_spawner" pkg="controller_manager" exec="spawner"
 		args="-t effort_controllers/JointGroupEffortController $(var controller)"/>
 
 	<node name="joint_state_broadcaster_spawner" pkg="controller_manager" exec="spawner"
 		args="-t joint_state_broadcaster/JointStateBroadcaster joint_state_broadcaster"/>
-	
+		
 	<include if="$(var gui)" file="$(find-pkg-share q2d_description)/launch/display.launch.xml">
 		<arg name="gui" value="false"/>
 	</include>
diff --git a/q2d_description/urdf/q2d.urdf b/q2d_description/urdf/q2d.urdf
index c3791a6..f2d2c50 100644
--- a/q2d_description/urdf/q2d.urdf
+++ b/q2d_description/urdf/q2d.urdf
@@ -4,15 +4,8 @@
 	<xacro:arg name="hardware" default="gazebo"/>
 	<xacro:property name="hardware" value="$(arg hardware)"/>
 
-	<link name="world"/>
-
 	<link name="origin_link"/>
 
-	<joint name="world_joint" type="fixed">
-		<parent link="world"/>
-		<child link="origin_link"/>
-	</joint>
-
 	<link name="base_link">
 		<inertial>
 			<origin xyz="0.074571214 -0.00008502114 0.039600060500000"/>
diff --git a/q2d_hardware/config/controller_manager.yaml b/q2d_hardware/config/controller_manager.yaml
index 17610f3..96f4c73 100644
--- a/q2d_hardware/config/controller_manager.yaml
+++ b/q2d_hardware/config/controller_manager.yaml
@@ -1,6 +1,6 @@
 controller_manager:
         ros__parameters:
-                update_rate: 500
+                update_rate: 100
                 use_sim_time: false
 
                 group_bypass:
diff --git a/q2d_hardware/launch/controller_manager.launch.xml b/q2d_hardware/launch/controller_manager.launch.xml
index e81ef36..153f699 100644
--- a/q2d_hardware/launch/controller_manager.launch.xml
+++ b/q2d_hardware/launch/controller_manager.launch.xml
@@ -25,5 +25,6 @@
 		<param name="use_sim_time" value="false"/>
 		<param name="update_rate" value="100"/>
 		<param from="$(find-pkg-share q2d_hardware)/config/controller_manager.yaml"/>
+		<remap from="controller_manager/commands" to="group_controller/commands"/>
 	</node>
 </launch>
diff --git a/q2d_hardware/src/q2d_system.cpp b/q2d_hardware/src/q2d_system.cpp
index 96f7ca6..5cef294 100644
--- a/q2d_hardware/src/q2d_system.cpp
+++ b/q2d_hardware/src/q2d_system.cpp
@@ -112,7 +112,7 @@ namespace q2d_hardware
 				return CallbackReturn::ERROR;
 			}
 		}
-		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_init");
+//		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_init");
 		return CallbackReturn::SUCCESS;
 	}
 
@@ -126,9 +126,9 @@ namespace q2d_hardware
 		{
 			param.aic_serial_port="/dev/ttyUSB"+std::string({char('0'+i),'\0'});
 			if((board_[i]=std::make_unique<aic>(param)) == NULL)
-			RCLCPP_ERROR_STREAM(rclcpp::get_logger("Q2dSystemHardware"),
+				RCLCPP_ERROR_STREAM(rclcpp::get_logger("Q2dSystemHardware"),
 					"Can not configure " << param.aic_serial_port << " for interfacing with joint " << i);
-			RCLCPP_WARN_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_configure: " << param.aic_serial_port);
+			RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"Q2D joint " << i+1 << " on "<< param.aic_serial_port);
 		}
 		return CallbackReturn::SUCCESS;
 	}
@@ -146,7 +146,7 @@ namespace q2d_hardware
 				hardware_interface::HW_IF_EFFORT,&effort_[i]));
 		}
 		
-		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"export_state_interface");
+//		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"export_state_interface");
 		return state_interfaces;
 	}
 
@@ -159,7 +159,7 @@ namespace q2d_hardware
 				hardware_interface::HW_IF_EFFORT,&command_[i]));
 		}
 		
-		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"export_command_interface");
+//		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"export_command_interface");
 		return command_interfaces;
 	}
 
@@ -174,25 +174,25 @@ namespace q2d_hardware
 		}
                 
 		/* activate hardware */
-		for(auto i=0u;i < board_.size()-1;i++) board_[i]->set_motor_voltage(0);
+		for(auto i=0u;i < board_.size();i++) board_[i]->set_motor_voltage(0);
 
-		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_activate");
+//		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_activate");
 		return CallbackReturn::SUCCESS;
 	}
 
 	hardware_interface::CallbackReturn Q2dSystemHardware::on_deactivate(const rclcpp_lifecycle::State &/*previous_state*/)
 	{
 		/* deactivate hardware */
-		for(auto i=0u;i < board_.size()-1;i++) board_[i]->~aic();
+		for(auto i=0u;i < board_.size();i++) board_[i]->~aic();
 
-		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_deactivate");
+//		RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_deactivate");
 		return CallbackReturn::SUCCESS;;
 	}
 
 	hardware_interface::return_type Q2dSystemHardware::read(const rclcpp::Time &/*time*/,const rclcpp::Duration &period)
 	{
 		/* read hardware */
-		for(auto i=0u;i < board_.size()-1;i++)
+		for(auto i=0u;i < board_.size();i++)
 		{
 			auto dp=board_[i]->read_displacement_sensors();
 			velocity_[i]=dp.joint_displacement/period.seconds()*N_[i];
@@ -207,7 +207,7 @@ namespace q2d_hardware
 	hardware_interface::return_type Q2dSystemHardware::write(const rclcpp::Time &/*time*/,const rclcpp::Duration &/*period*/)
 	{
 		/* write hardware */
-		for(auto i=0u;i < board_.size()-1;i++)
+		for(auto i=0u;i < board_.size();i++)
 			board_[i]->set_motor_voltage(command_[i]);
 //			board_[i]->set_motor_voltage(N_[i]*Ra_[i]/Kt_[i]*command_[i]+Ka_[i]/N_[i]*velocity_[i]);