Commit 48e8b906 authored by Christian Sponfeldner's avatar Christian Sponfeldner

refactor emros teleop calibration

parent 3443cbe8
......@@ -104,7 +104,9 @@ void SeppCalibTeleop::setParam()
mNodeHandler.setParam("sepp_teleop/rangeY/min", mParam.rangeY.min);
mNodeHandler.setParam("sepp_teleop/rangeWz/max", mParam.rangeWz.max);
mNodeHandler.setParam("sepp_teleop/rangeWz/min", mParam.rangeWz.min);
system("rosparam dump ~/sepp_teleop.yaml /sepp_teleop");
int ignored __attribute__((unused));
ignored = system("rosparam dump ~/sepp_teleop.yaml /sepp_teleop");
}
void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
......@@ -117,7 +119,7 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
mState = States::XAxisMaxWait;
break;
case States::XAxisMaxWait:
if (joy->axes.at(0) > mThreshold)
if (static_cast<double>(joy->axes.at(0)) > mThreshold)
{
mIterator = 0;
mTemp = 0;
......@@ -125,7 +127,7 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
}
break;
case States::XAxisMaxCalc:
mTemp = mTemp + joy->axes.at(0);
mTemp = mTemp + static_cast<double>(joy->axes.at(0));
mIterator++;
if (mIterator > 50)
{
......@@ -140,7 +142,7 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
break;
case States::XAxisMinWait:
if (joy->axes.at(0) < -mThreshold)
if (static_cast<double>(joy->axes.at(0)) < -mThreshold)
{
mIterator = 0;
mTemp = 0;
......@@ -149,7 +151,7 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
break;
case States::XAxisMinCalc:
mTemp = mTemp + joy->axes.at(0);
mTemp = mTemp + static_cast<double>(joy->axes.at(0));
mIterator++;
if (mIterator > 50)
{
......@@ -212,7 +214,7 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
break;
case States::WzAxisMaxWait:
if (joy->axes.at(5) > mThreshold)
if (static_cast<double>(joy->axes.at(5)) > mThreshold)
{
mIterator = 0;
mTemp = 0;
......@@ -221,7 +223,7 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
break;
case States::WzAxisMaxCalc:
mTemp = mTemp + joy->axes.at(5);
mTemp = mTemp + static_cast<double>(joy->axes.at(5));
mIterator++;
if (mIterator > 50)
{
......@@ -236,7 +238,7 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
break;
case States::WzAxisMinWait:
if (joy->axes.at(5) < -mThreshold)
if (static_cast<double>(joy->axes.at(5)) < -mThreshold)
{
mIterator = 0;
mTemp = 0;
......@@ -245,7 +247,7 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
break;
case States::WzAxisMinCalc:
mTemp = mTemp + joy->axes.at(5);
mTemp = mTemp + static_cast<double>(joy->axes.at(5));
mIterator++;
if (mIterator > 50)
{
......@@ -259,6 +261,9 @@ void SeppCalibTeleop::doCalibration(const sensor_msgs::Joy::ConstPtr& joy)
setParam();
mCalculationDone = true;
break;
default:
ROS_ERROR("Calib-Teleop: Case not implemented");
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment