Fix controller loading for hardware execution.

diff --git a/q2d_bringup/config/group_bypass.yaml b/q2d_bringup/config/group_bypass.yaml
index 61eef34..0413e10 100644 (file)
--- a/q2d_bringup/config/group_bypass.yaml
+++ b/q2d_bringup/config/group_bypass.yaml
@@ -1,4 +1,4 @@
-group_controller:
+group_bypass:
         ros__parameters:
                 joints: 
                         - shoulder_active_joint

diff --git a/q2d_bringup/launch/hardware.launch.xml b/q2d_bringup/launch/hardware.launch.xml
index 8a47983..38ef83c 100644 (file)
--- a/q2d_bringup/launch/hardware.launch.xml
+++ b/q2d_bringup/launch/hardware.launch.xml
@@ -26,11 +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">
                <arg name="config" value="$(var config)"/>
                <arg name="use_sim_time" value="false"/>
        </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"/>

diff --git a/q2d_hardware/CMakeLists.txt b/q2d_hardware/CMakeLists.txt
index 9331a8a..0aa76be 100644 (file)
--- a/q2d_hardware/CMakeLists.txt
+++ b/q2d_hardware/CMakeLists.txt
@@ -45,7 +45,7 @@ install(
   RUNTIME DESTINATION bin
 )
 
-install(DIRECTORY launch
+install(DIRECTORY launch config
        DESTINATION share/${PROJECT_NAME}
 )
 

diff --git a/q2d_hardware/config/controller_manager.yaml b/q2d_hardware/config/controller_manager.yaml
new file mode 100644 (file)
index 0000000..17610f3
--- /dev/null
+++ b/q2d_hardware/config/controller_manager.yaml
@@ -0,0 +1,11 @@
+controller_manager:
+        ros__parameters:
+                update_rate: 500
+                use_sim_time: false
+
+                group_bypass:
+                        type: effort_controllers/JointGroupEffortController
+
+                joints:
+                        - shoulder_active_joint
+                        - elbow_active_joint

diff --git a/q2d_hardware/launch/controller_manager.launch.xml b/q2d_hardware/launch/controller_manager.launch.xml
index 7e1e66c..e81ef36 100644 (file)
--- a/q2d_hardware/launch/controller_manager.launch.xml
+++ b/q2d_hardware/launch/controller_manager.launch.xml
@@ -23,6 +23,7 @@
        <node name="controller_manager" pkg="controller_manager" exec="ros2_control_node">
                <param name="robot_description" value="$(command 'xacro $(find-pkg-share q2d_description)/urdf/q2d.urdf hardware:=real_robot')" type="str"/>
                <param name="use_sim_time" value="false"/>
-               <param name="update_rate" value="1000"/>
+               <param name="update_rate" value="100"/>
+               <param from="$(find-pkg-share q2d_hardware)/config/controller_manager.yaml"/>
        </node>
 </launch>

diff --git a/q2d_hardware/src/q2d_system.cpp b/q2d_hardware/src/q2d_system.cpp
index fddb645..96f7ca6 100644 (file)
--- a/q2d_hardware/src/q2d_system.cpp
+++ b/q2d_hardware/src/q2d_system.cpp
@@ -53,6 +53,8 @@ namespace q2d_hardware
                Ra_[0]=11.5;
                Ra_[1]=2.32;
                N_.resize(info_.joints.size());
+               N_[0]=0.01;
+               N_[1]=0.02;
 
                for(const hardware_interface::ComponentInfo & joint : info_.joints)
                {
@@ -110,6 +112,7 @@ namespace q2d_hardware
                                return CallbackReturn::ERROR;
                        }
                }
+               RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"on_init");
                return CallbackReturn::SUCCESS;
        }
 
@@ -121,10 +124,11 @@ namespace q2d_hardware
                
                for(auto i=0u;i < board_.size();i++)
                {
-                       param.aic_serial_port="/dev/tty/USB"+ ('0'+i);
+                       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);
                }
                return CallbackReturn::SUCCESS;
        }
@@ -139,8 +143,10 @@ namespace q2d_hardware
                        state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name,
                                hardware_interface::HW_IF_VELOCITY,&velocity_[i]));
                        state_interfaces.emplace_back(hardware_interface::StateInterface(info_.joints[i].name,
-                               hardware_interface::HW_IF_EFFORT,&command_[i]));
+                               hardware_interface::HW_IF_EFFORT,&effort_[i]));
                }
+               
+               RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"export_state_interface");
                return state_interfaces;
        }
 
@@ -152,6 +158,8 @@ namespace q2d_hardware
                        command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name,
                                hardware_interface::HW_IF_EFFORT,&command_[i]));
                }
+               
+               RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"export_command_interface");
                return command_interfaces;
        }
 
@@ -166,39 +174,44 @@ namespace q2d_hardware
                }
                 
                /* activate hardware */
-               for(auto i=0u;i < board_.size();i++) board_[i]->set_motor_voltage(0);
-                        
+               for(auto i=0u;i < board_.size()-1;i++) board_[i]->set_motor_voltage(0);
+
+               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();i++) board_[i]->~aic();
+               for(auto i=0u;i < board_.size()-1;i++) board_[i]->~aic();
 
+               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();i++)
+               for(auto i=0u;i < board_.size()-1;i++)
                {
                        auto dp=board_[i]->read_displacement_sensors();
-                       velocity_[i]=(dp.joint_displacement-position_[i])/period.seconds();
-                       position_[i]=dp.joint_displacement;
+                       velocity_[i]=dp.joint_displacement/period.seconds()*N_[i];
+                       position_[i]+=dp.joint_displacement*N_[i];
                        effort_[i]=command_[i];
                }
-               
+
+//             RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"read");             
                return hardware_interface::return_type::OK;
        }
 
        hardware_interface::return_type Q2dSystemHardware::write(const rclcpp::Time &/*time*/,const rclcpp::Duration &/*period*/)
        {
                /* write hardware */
-               for(auto i=0u;i < board_.size();i++)
-                       board_[i]->set_motor_voltage(N_[i]*Ra_[i]/Kt_[i]*command_[i]+Ka_[i]/N_[i]*velocity_[i]);
+               for(auto i=0u;i < board_.size()-1;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]);
 
+//             RCLCPP_INFO_STREAM(rclcpp::get_logger("Q2dSystemHardware"),"write");
                return hardware_interface::return_type::OK;
        }