Merge branch 'jsk-ros-pkg:master' into develop/mujoco

.github/PULL_REQUEST_TEMPLATE.md

@@ -0,0 +1,24 @@
+### What is this
+
+Please briefly explain the purpose and the result of this PR.
+
+### Details
+
+Please explain the contribution of this PR in detail.
+
+- commit1:
+- commit2:
+- commit3:
+
+### TODO
+
+If there is still some work to do. Plase list up as follows:
+
+- [ ] todo1
+- [ ] todo2
+- [ ] todo3
+
+
+### Video
+
+Please attach video for better understanding if possible

aerial_robot_estimation/include/aerial_robot_estimation/sensor/vo.h

@@ -75,6 +75,7 @@ namespace sensor_plugin
     ros::Timer  servo_control_timer_;
 
     /* ros param */
+    double throttle_rate_;
     double level_pos_noise_sigma_;
     double z_pos_noise_sigma_;
     double vel_noise_sigma_;

aerial_robot_estimation/src/sensor/vo.cpp

@@ -44,9 +44,6 @@ namespace
   tf::Transform prev_sensor_tf;
   tf::Vector3 baselink_omega;
   tf::Matrix3x3 baselink_r;
-
-  //debug
-  double sample_interval = 0.02; //50 hz
 }
 
 namespace sensor_plugin
@@ -99,8 +96,6 @@ namespace sensor_plugin
 
   void VisualOdometry::voCallback(const nav_msgs::Odometry::ConstPtr & vo_msg)
   {
-    //if(vo_msg->header.stamp.toSec() - prev_timestamp_ < sample_interval) return;
-
     /* only do egmotion estimate mode */
     if(!getFuserActivate(aerial_robot_estimation::EGOMOTION_ESTIMATE))
       {
@@ -139,9 +134,19 @@ namespace sensor_plugin
     tf::Transform raw_sensor_tf;
     tf::poseMsgToTF(vo_msg->pose.pose, raw_sensor_tf); // motion update
 
-    curr_timestamp_ = vo_msg->header.stamp.toSec() + delay_; //temporal update
+    /* temporal update */
+    curr_timestamp_ = vo_msg->header.stamp.toSec() + delay_;
     reference_timestamp_ = curr_timestamp_;
 
+    /* throttle message */
+    if(throttle_rate_ > 0)
+      {
+        if (curr_timestamp_ - prev_timestamp_ < 1 / throttle_rate_)
+          {
+            return;
+          }
+      }
+
     if(getStatus() == Status::INACTIVE)
       {
         /* for z */
@@ -598,6 +603,7 @@ namespace sensor_plugin
     getParam<bool>("z_vel_mode", z_vel_mode_, false);
     getParam<bool>("z_no_delay", z_no_delay_, false);
     getParam<bool>("outdoor_no_vel_time_sync", outdoor_no_vel_time_sync_, false);
+    getParam<double>("throttle_rate", throttle_rate_, 0.0);
     getParam<double>("level_pos_noise_sigma", level_pos_noise_sigma_, 0.01 );
     getParam<double>("z_pos_noise_sigma", z_pos_noise_sigma_, 0.01 );
     getParam<double>("vel_noise_sigma", vel_noise_sigma_, 0.05 );

aerial_robot_simulation/include/aerial_robot_simulation/rotor_handle.h

@@ -77,6 +77,7 @@ namespace hardware_interface
 
       motor_nh.param("rotor_force_noise", rotor_force_noise_, 0.0); // N
       motor_nh.param("dual_rotor_moment_noise", dual_rotor_moment_noise_, 0.0);
+      motor_nh.param("speed_rate", speed_rate_, 1.0); // rad/s/N , this is a virtual linear rate of speed-f
     }
 
     inline std::string getName() const {return name_;}
@@ -102,13 +103,19 @@ namespace hardware_interface
     }
     inline void setCommand(double command); //no implement here
 
+    inline double getSpeed() const
+    {
+      return *force_ * speed_rate_;
+    }
+
   private:
     std::string name_;
     boost::shared_ptr<double> force_;
     int direction_;
     double f_pwm_rate_;
     double f_pwm_offset_;
     double m_f_rate_;
+    double speed_rate_;
     double pwm_rate_;
     double max_pwm_;
 

aerial_robot_simulation/include/aerial_robot_simulation/spinal_interface.h

@@ -113,6 +113,8 @@ namespace rotor_limits_interface
      */
     void enforceLimits(const ros::Duration& /* period */)
     {
+      if(jh_.getForce() == 0) return;
+
       /* because of "inline double setForce(double force)    {*force_ = force;}", we can change the value with same address */
       jh_.setForce(internal::saturate(jh_.getForce(), min_force_, max_force_));
     }

aerial_robot_simulation/src/aerial_robot_hw_sim.cpp

@@ -376,6 +376,7 @@ namespace gazebo_ros_control
             auto  torque = rotor.getTorque();
             parent_link->AddRelativeTorque(gazebo::math::Vector3(torque.x(), torque.y(), torque.z()));
 #endif
+            sim_rotors_.at(j)->SetVelocity(0, rotor.getSpeed());
           }
       }
   }

robots/hydrus/urdf/link.urdf.xacro

@@ -212,7 +212,7 @@
         </xacro:unless>
       </xacro:if>
       <xacro:unless value="${tilt_mode == 1}">
-        <origin rpy="0 0 0" xyz="${link_length* 0.5} 0 0"/>
+        <origin rpy="0 0 0" xyz="${link_length* 0.5} 0 0.013"/>
       </xacro:unless>
       <axis xyz="0 0 ${rotor_direction}"/>
     </joint>
@@ -233,6 +233,17 @@
           <cylinder length="0.1" radius="${protector_radius + 0.01}"/>
         </geometry>
       </collision>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0.032"/>
+        <geometry>
+          <xacro:if value="${rotor_direction == 1}">
+            <mesh filename="package://hydrus/urdf/mesh/propeller/T-motor-CF-14inch-CCW.dae"/>
+          </xacro:if>
+          <xacro:if value="${rotor_direction == -1}">
+            <mesh filename="package://hydrus/urdf/mesh/propeller/T-motor-CF-14inch-CW.dae"/>
+          </xacro:if>
+        </geometry>
+      </visual>
     </link>
     <xacro:damping_factor link ="thrust${self}"/>