From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sat, 4 Feb 2023 23:09:32 +0000 (-0300)
Subject: Fix indentation.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=a95726ccc249249936866133469cb8dbbc85de60;p=q2d.git

Fix indentation.
---

diff --git a/q2d_teleop/src/q2d_teleop_rviz.cpp b/q2d_teleop/src/q2d_teleop_rviz.cpp
index 265cf8f..88626f6 100644
--- 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;