Merge pull request #29 from Brazilian-Institute-of-Robotics/thruster_feedback

Add thruster feedback output port

Author: doudou

rock_gazebo.orogen

@@ -70,6 +70,8 @@ task_context "ThrusterTask" do
 
     # Thruster input port
     input_port "thrusters_cmd", "/base/samples/Joints"
+    # Thruster status port
+    output_port "joint_samples", "/base/samples/Joints"
 end
 
 task_context "LaserScanTask" do

tasks/ThrusterTask.cpp

@@ -68,6 +68,8 @@ void ThrusterTask::updateHook()
             gzmsg << "ThrusterTask: publisher has no connections. Going into exception" << endl;
             exception(NO_TOPIC_CONNECTION);
         }
+        jointsCMD.time = base::Time::now();
+        _joint_samples.write( jointsCMD );
     }
 }