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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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 (file)
--- 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]);