Added servo startup_value property for safer startup pose

description/urdf/hexapod_robot_ros2_control.xacro

@@ -27,13 +27,14 @@
         </joint>
     </xacro:macro>
 
-    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel counts_intersect counts_slope counts_min counts_max">
+    <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel counts_intersect counts_slope counts_min counts_max startup_value">
         <param name="${name}_joint_i2c_address">${i2c_address}</param>
         <param name="${name}_joint_channel">${channel}</param>
         <param name="${name}_joint_counts_intersect">${counts_intersect}</param>
         <param name="${name}_joint_counts_slope">${counts_slope}</param>
         <param name="${name}_joint_counts_min">${counts_min}</param>
         <param name="${name}_joint_counts_max">${counts_max}</param>
+        <param name="${name}_joint_startup_value">${startup_value}</param>
     </xacro:macro>
 
     <ros2_control name="Hexapod" type="system">
@@ -41,29 +42,29 @@
         <hardware>
             <plugin>hexapod_robot/HexapodRobotHardwareInterface</plugin>
             <param name="i2c_device">/dev/i2c-1</param>
-            <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13" counts_intersect="386" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="189" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13" counts_intersect="386" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.4"/>
+            <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="189" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
 
-            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"  counts_intersect="327" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="317" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="227" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9"  counts_intersect="327" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="317" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.4"/>
+            <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="227" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>        
             
-            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"  counts_intersect="237" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"  counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"  counts_intersect="267" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2"  counts_intersect="237" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1"  counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.4"/>
+            <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0"  counts_intersect="267" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>            
             
-            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="451" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="487" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="451" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.4"/>
+            <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="487" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>            
             
-            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"  counts_intersect="357" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"  counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"  counts_intersect="437" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6"  counts_intersect="357" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5"  counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.4"/>
+            <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4"  counts_intersect="437" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>            
             
-            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"  counts_intersect="255" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"  counts_intersect="335" counts_slope="129" counts_min="125" counts_max="550"/>
-            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"  counts_intersect="444" counts_slope="129" counts_min="125" counts_max="550"/>
+            <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2"  counts_intersect="255" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+            <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1"  counts_intersect="335" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.4"/>
+            <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0"  counts_intersect="444" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
         </hardware>
 
         <xacro:create_leg_joints number="1"/>    

hardware/hexapod_robot_hwi.cpp

@@ -35,7 +35,6 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
     servos_powered_ = false;
 
     for (const hardware_interface::ComponentInfo &joint: info_.joints) {
-        joint_commands_[joint.name] = 0.0;
 
         std::string i2c_address_parameter = joint.name + "_i2c_address";
         std::string channel_parameter = joint.name + "_channel";
@@ -43,6 +42,7 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
         std::string counts_slope_parameter = joint.name + "_counts_slope";
         std::string counts_min_parameter = joint.name + "_counts_min";
         std::string counts_max_parameter = joint.name + "_counts_max";
+        std::string startup_value_parameter = joint.name + "_startup_value";
 
         PWMOutput pwm_output;
         pwm_output.i2c_address      = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
@@ -51,6 +51,7 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
         pwm_output.counts_slope     = stod(info_.hardware_parameters[counts_slope_parameter]);
         pwm_output.counts_min       = stod(info_.hardware_parameters[counts_min_parameter]);
         pwm_output.counts_max       = stod(info_.hardware_parameters[counts_max_parameter]);
+        joint_commands_[joint.name] = stod(info_.hardware_parameters[startup_value_parameter]);
 
         pca_available_[pwm_output.i2c_address] = false;
         joint_pwm_mapping_[joint.name] = pwm_output;
@@ -60,7 +61,7 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
 
 
     i2c_available_ = i2c_open_(info_.hardware_parameters["i2c_device"]);
-    pca_initialize_();
+    if(i2c_available_) pca_initialize_();
 
     return hardware_interface::CallbackReturn::SUCCESS;
 }