Remove extra spaces

utecrobotics · Feb 3, 2018 · 97e026f · 97e026f
1 parent f90a1a4
commit 97e026f
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 40 deletions.
diff --git a/src/markers.cpp b/src/markers.cpp
@@ -1,6 +1,6 @@
-/*                                                                               
+/*
  * Copyright 2016
- * J.Avalos, S.Cortez, O.Ramos. 
+ * J.Avalos, S.Cortez, O.Ramos.
  * Universidad de Ingenieria y Tecnologia - UTEC
  *
  * This file is part of nao_kinect_teleop.
@@ -23,14 +23,14 @@
 Marker::Marker(ros::NodeHandle& nh)
 {
   nh.param<std::string>("reference_frame", reference_frame_, "/map");
-  marker_pub_ = 
+  marker_pub_ =
     nh.advertise<visualization_msgs::Marker>("visualization_marker", 10);
 }
 
 
 unsigned int BallMarker::id_=0;
 
-BallMarker::BallMarker(ros::NodeHandle& nh, 
+BallMarker::BallMarker(ros::NodeHandle& nh,
                        double color[3],
                        const double& scale)
   : Marker(nh)

diff --git a/src/nao_arms_teleop.cpp b/src/nao_arms_teleop.cpp
@@ -1,6 +1,6 @@
-/*                                                                               
+/*
  * Copyright 2016
- * J.Avalos, S.Cortez, O.Ramos. 
+ * J.Avalos, S.Cortez, O.Ramos.
  * Universidad de Ingenieria y Tecnologia - UTEC
  *
  * This file is part of nao_kinect_teleop.
@@ -22,19 +22,22 @@
   -----------------------------------------
 */
 
+#include <cmath>
 #include <ros/ros.h>
 #include <ros/package.h>
 #include <sensor_msgs/JointState.h>
 #include <naoqi_bridge_msgs/JointAnglesWithSpeed.h>
+
 #include <robot-model/robot-model.hpp>
 #include <osik-control/math-tools.hpp>
 #include <osik-control/kine-task.hpp>
 #include <osik-control/kine-task-pose.hpp>
 #include <osik-control/kine-solver-WQP.hpp>
+
 #include <nao_kinect_teleop/robotSpecifics.h>
 #include <nao_kinect_teleop/tools.hpp>
 #include <kinect_msgs/BodyArray.h>
-#include <cmath>
+
 
 using namespace osik;
 
@@ -115,7 +118,7 @@ int main(int argc, char **argv)
   {
     if (niter++ == max_iter)
     {
-      std::cerr << "Initial sensed joint configuration does not have " 
+      std::cerr << "Initial sensed joint configuration does not have "
                 << ndof_red << "degrees of freedom, stopping ..." << std::endl;
       exit(0);
     }
@@ -138,7 +141,7 @@ int main(int argc, char **argv)
 
   // Initialize names of command joints
   for (unsigned int i=0; i<ndof_full; ++i)
-  { 
+  {
     if (ridx[i] != 100)
      jcmd.joint_names[ridx[i]] = jnames[i];
   }
@@ -153,7 +156,7 @@ int main(int argc, char **argv)
   KineSolverWQP solver(robot, qsensed, dt);
   solver.setJointLimits(qmin, qmax, dqmax);
 
- 
+
   //KineTask* taskrh = new KineTaskPose(robot, RGRIPPER, "position");
   KineTask* taskrh = new KineTaskPose(robot, RGRIPPER, "position");
   taskrh->setGain(300.0);
@@ -168,7 +171,7 @@ int main(int argc, char **argv)
   Eigen::VectorXd P_right_elbow;
   Eigen::VectorXd P_left_wrist;
   Eigen::VectorXd P_left_elbow;
-  
+
   solver.pushTask(taskrh);
   solver.pushTask(tasklh);
   solver.pushTask(taskre);
@@ -188,10 +191,10 @@ int main(int argc, char **argv)
       //Datos del Nao
       double L1 = 0.108; //Del hombro al codo
       double L2 = 0.111; //Del codo a la mano
-      
+
       //Recepcion del brazo derecho
       for (unsigned k=0;k<(P.size()/2);k++)
-      {    
+      {
         P[k].resize(3);
         //A partir de eso P[0][k]=(0,0,0)
         P[k][0] = (-kpoints.getPoints()->body[k].z)-(-kpoints.getPoints()->body[0].z);
@@ -228,12 +231,12 @@ int main(int argc, char **argv)
 
       // Recepcion del brazo izquierdo
       for (unsigned k=(P.size()/2);k<P.size();k++)
-      {    
+      {
 	P[k].resize(3);
 	P[k][0] = (-kpoints.getPoints()->body[k].z)-(-kpoints.getPoints()->body[3].z);
 	P[k][1] = (-kpoints.getPoints()->body[k].x)-(-kpoints.getPoints()->body[3].x);
 	P[k][2] = (kpoints.getPoints()->body[k].y)-(kpoints.getPoints()->body[3].y);
-      } 
+      }
       //Hallamos el modulo M1 de P4
       double M3 = sqrt(pow(P[4][0], 2.0) + pow(P[4][1], 2.0) + pow(P[4][2], 2.0));
       //Hallamos el modulo M2 de (P2-P1)
@@ -263,32 +266,32 @@ int main(int argc, char **argv)
       P_left_wrist.resize(3);
       P_right_elbow.resize(3);
       P_left_elbow.resize(3);
-      
+
       //Elbow Izquierdo
       P_left_elbow[0] = P[1][0];
       P_left_elbow[1] = P[1][1];
       P_left_elbow[2] = P[1][2];
-        
+
       //Left hand
       P_left_wrist[0] = P[2][0];
       P_left_wrist[1] = P[2][1];
       P_left_wrist[2] = P[2][2];
-        
+
       //Right elbow
       P_right_elbow[0] = P[4][0];
       P_right_elbow[1] = P[4][1];
       P_right_elbow[2] = P[4][2];
-        
+
       //Right hand
       P_right_wrist[0] = P[5][0];
       P_right_wrist[1] = P[5][1];
       P_right_wrist[2] = P[5][2];
-        
+
       taskle->setDesiredValue(P_left_elbow);
       tasklh->setDesiredValue(P_left_wrist);
       taskre->setDesiredValue(P_right_elbow);
       taskrh->setDesiredValue(P_right_wrist);
-    
+
       solver.getPositionControl(qsensed, qdes);
       jcmd.header.stamp = ros::Time::now();
       reducedJointModel(jcmd.joint_angles, qdes);
@@ -304,12 +307,12 @@ int main(int argc, char **argv)
 	jcmd.joint_angles[25]=0.0;
       }
       pub.publish(jcmd);
-      
+
       qsensed = qdes;
     }
     ros::spinOnce();
     rate.sleep();
-    
+
   }
   //#######################################################3
   return 0;

diff --git a/src/tools.cpp b/src/tools.cpp
@@ -1,6 +1,6 @@
-/*                                                                               
+/*
  * Copyright 2016
- * J.Avalos, S.Cortez, O.Ramos. 
+ * J.Avalos, S.Cortez, O.Ramos.
  * Universidad de Ingenieria y Tecnologia - UTEC
  *
  * This file is part of nao_kinect_teleop.

diff --git a/src/windows/kinect_acquisition.cpp b/src/windows/kinect_acquisition.cpp
@@ -1,6 +1,6 @@
-/*                                                                               
+/*
  * Copyright 2016
- * J.Avalos, S.Cortez 
+ * J.Avalos, S.Cortez
  * Universidad de Ingenieria y Tecnologia - UTEC
  *
  * This file is part of nao_kinect_teleop.
@@ -38,16 +38,16 @@ int main()
   HRESULT hResult = S_OK;
   hResult = GetDefaultKinectSensor(&pSensor);
   hResult = pSensor->Open();
-  
+
   IBodyFrameSource* pBodySource;
   hResult = pSensor->get_BodyFrameSource(&pBodySource);
-  
+
   IBodyFrameReader* pBodyReader;
   hResult = pBodySource->OpenReader(&pBodyReader);
-  
+
   ICoordinateMapper* pCoordinateMapper;
   hResult = pSensor->get_CoordinateMapper(&pCoordinateMapper);
-  
+
   //Inicializacion del envio de datos (Ubuntu)
   ros::NodeHandle nh;
   char *ros_master = "10.100.144.116";
@@ -69,31 +69,31 @@ int main()
   while (1)
   {
     Sleep(100);
-    
+
     IBodyFrame* pBodyFrame = nullptr;
     hResult = pBodyReader->AcquireLatestFrame(&pBodyFrame);
     if (SUCCEEDED(hResult)){
       IBody* pBody[BODY_COUNT] = { 0 };
       hResult = pBodyFrame->GetAndRefreshBodyData(BODY_COUNT, pBody);
-      
+
       if (pBodyFrame != nullptr)
       {
         for (int count = 0; count < BODY_COUNT; count++)
         {
           BOOLEAN bTracked = false;
           hResult = pBody[count]->get_IsTracked(&bTracked);
-          
+
           if (SUCCEEDED(hResult) && bTracked)
           {
             Joint joint[JointType::JointType_Count];
             hResult = pBody[count]->GetJoints(JointType::JointType_Count, joint);
-            
+
             if (SUCCEEDED(hResult))
             {
               // Estado de mano izquierda
               HandState leftHandState = HandState::HandState_Unknown;
               hResult = pBody[count]->get_HandLeftState(&leftHandState);
-              
+
               if (SUCCEEDED(hResult)){
                 if (leftHandState == HandState::HandState_Open){
                   body_msg.left_hand.data = true;
@@ -102,11 +102,11 @@ int main()
                   body_msg.left_hand.data = false;
                 }
               }
-              
+
               // Estado de mano derecha
               HandState rightHandState = HandState::HandState_Unknown;
               hResult = pBody[count]->get_HandRightState(&rightHandState);
-              
+
               if (SUCCEEDED(hResult)){
                 if (rightHandState == HandState::HandState_Open){
                   body_msg.right_hand.data = true;
@@ -115,12 +115,12 @@ int main()
                   body_msg.right_hand.data = false;
                 }
               }
-              
+
               //Articulaciones
               for (int i = 0; i < 6; i++){
                 if (joint[orden[i]].TrackingState != TrackingState::TrackingState_NotTracked){
                   CameraSpacePoint cameraSpacePoint = joint[orden[i]].Position;
-                  
+
                   body_msg.body[i].x = joint[orden[i]].Position.X;
                   body_msg.body[i].y = joint[orden[i]].Position.Y;
                   body_msg.body[i].z = joint[orden[i]].Position.Z;