Fix indentation.

diff --git a/q2d_teleop/src/q2d_teleop_rviz.cpp b/q2d_teleop/src/q2d_teleop_rviz.cpp
index 265cf8f..88626f6 100644 (file)
--- a/q2d_teleop/src/q2d_teleop_rviz.cpp
+++ b/q2d_teleop/src/q2d_teleop_rviz.cpp
@@ -58,16 +58,16 @@ Q2dTeleop::Q2dTeleop(void): Node("Q2d_teleop_rviz"), q_(2)
 {
        clickSub_=create_subscription<geometry_msgs::msg::PointStamped>("clicked_point",100,std::bind(&Q2dTeleop::clickCB,this,std::placeholders::_1));
        shoulderCmdPub_=create_publisher<std_msgs::msg::Float64>("shoulder_controller/command",100);
-        elbowCmdPub_=create_publisher<std_msgs::msg::Float64>("elbow_controller/command",100);
+       elbowCmdPub_=create_publisher<std_msgs::msg::Float64>("elbow_controller/command",100);
        
-        rclcpp::QoS qos(rclcpp::KeepLast(1));
-        qos.transient_local();
-        auto robotDescriptionSubscriber_=create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&Q2dTeleop::robotDescriptionCB,this,std::placeholders::_1));
-        while(robotDescription_.empty())
-        {
+       rclcpp::QoS qos(rclcpp::KeepLast(1));
+       qos.transient_local();
+       auto robotDescriptionSubscriber_=create_subscription<std_msgs::msg::String>("robot_description",qos,std::bind(&Q2dTeleop::robotDescriptionCB,this,std::placeholders::_1));
+       while(robotDescription_.empty())
+       {
                 RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(get_logger(),*get_clock(),1000,"Waiting for robot model on /robot_description.");
                 rclcpp::spin_some(get_node_base_interface());
-        }
+       }
 
        KDL::Tree tree;
        if (!kdl_parser::treeFromString(robotDescription_,tree))
@@ -82,8 +82,8 @@ Q2dTeleop::Q2dTeleop(void): Node("Q2d_teleop_rviz"), q_(2)
        goal_.p.x(0.61);
        goal_.p.z(0.1477);
        
-        Eigen::Matrix<double,6,1> L;
-        L << 1.0 , 1.0 , 1.0, 0.01, 0.01, 0.01;
+       Eigen::Matrix<double,6,1> L;
+       L << 1.0 , 1.0 , 1.0, 0.01, 0.01, 0.01;
 
        // A copy of chain_ is not created inside!
        ikSolverPos_=new KDL::ChainIkSolverPos_LMA(chain_,L);
@@ -107,7 +107,7 @@ void Q2dTeleop::clickCB(const geometry_msgs::msg::PointStamped::SharedPtr click)
 
 void Q2dTeleop::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription)
 {
-        robotDescription_=robotDescription->data;
+       robotDescription_=robotDescription->data;
 }
 
 #define KDL_IK
@@ -118,34 +118,34 @@ void Q2dTeleop::publish(void)
        int error=0;
        if((error=ikSolverPos_->CartToJnt(q_,goal_,q_out)) < 0)
                RCLCPP_ERROR_STREAM(get_logger(),"Failed to compute invere kinematics: (" << error << ") "
-                       << ikSolverPos_->strError(error));
-        q_=q_out;
+                       << ikSolverPos_->strError(error));
+       q_=q_out;
 #else
        // Algebric inverse kinematics
-        const double l1=0.343;
-        const double l2=0.267;
-        double c1=(sqr(goal_.p.x())+sqr(goal_.p.y())-sqr(l1)-sqr(l2))/2/l1/l2;
-        if(c1 >= 0.0 && c1 <= 1.0)
-        {
-                double s1a=sqrt(1-sqr(c1));
-                double s1b=-s1a;
-                double q1a=atan2(s1a,c1);
-                double q1b=atan2(s1b,c1);
+       const double l1=0.343;
+       const double l2=0.267;
+       double c1=(sqr(goal_.p.x())+sqr(goal_.p.y())-sqr(l1)-sqr(l2))/2/l1/l2;
+       if(c1 >= 0.0 && c1 <= 1.0)
+       {
+               double s1a=sqrt(1-sqr(c1));
+               double s1b=-s1a;
+               double q1a=atan2(s1a,c1);
+               double q1b=atan2(s1b,c1);
         
-                double q0a=atan2(goal_.p.y(),goal_.p.x())-atan2(l2*s1a,l1+l2*c1);
-                double q0b=atan2(goal_.p.y(),goal_.p.x())-atan2(l2*s1b,l1+l2*c1);
+               double q0a=atan2(goal_.p.y(),goal_.p.x())-atan2(l2*s1a,l1+l2*c1);
+               double q0b=atan2(goal_.p.y(),goal_.p.x())-atan2(l2*s1b,l1+l2*c1);
         
-                if((fabs(q_(0)-q0a) < fabs(q_(0)-q0b)) || (cos(q0a) > 0.0))
-                {
-                        q_(0)=q0a;
-                        q_(1)=q1a;
-                }
-                else
-                {
-                        q_(0)=q0b;
-                        q_(1)=q1b;
-                }
-        }
+               if((fabs(q_(0)-q0a) < fabs(q_(0)-q0b)) || (cos(q0a) > 0.0))
+               {
+                       q_(0)=q0a;
+                       q_(1)=q1a;
+               }
+               else
+               {
+                       q_(0)=q0b;
+                       q_(1)=q1b;
+               }
+       }
 #endif
 
        std_msgs::msg::Float64 shoulderCmd;