Commit 860c9f63 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Remove sot_control_mode.

parent 830b2b81
Pipeline #4684 passed with stage
in 42 seconds
......@@ -569,46 +569,41 @@ namespace sot_controller
{
std::string scontrol_mode;
static const std::string seffort("EFFORT"),svelocity("VELOCITY"),sposition("POSITION");
static const std::string str_control_mode[2] =
{"ros_control_mode","sot_control_mode"};
static const std::string ros_control_mode = "ros_control_mode";
/// Read the list of control_mode
ros::NodeHandle rnh_ns("/sot_controller/control_mode/"+joint_name);
for(unsigned int i=0;i<2;i++)
ControlMode joint_control_mode;
if (!rnh_ns.getParam(ros_control_mode,scontrol_mode))
{
ControlMode joint_control_mode;
if (!rnh_ns.getParam(str_control_mode[i],scontrol_mode))
{
ROS_ERROR("%d No %s for %s - We found %s",i,
str_control_mode[i].c_str(),
joint_name.c_str(),
scontrol_mode.c_str());
return false;
}
if (scontrol_mode==sposition)
joint_control_mode=POSITION;
else if (scontrol_mode==svelocity)
joint_control_mode=VELOCITY;
else if (scontrol_mode==seffort)
joint_control_mode=EFFORT;
else {
ROS_ERROR("%d %s for %s not understood. Expected %s, %s or %s. Got %s",i,
str_control_mode[i].c_str(),
joint_name.c_str(),
sposition.c_str(),
svelocity.c_str(),
seffort.c_str(),
scontrol_mode.c_str());
return false;
}
if (i==0)
aJointSotHandle.ros_control_mode = joint_control_mode;
else if (i==1)
aJointSotHandle.sot_control_mode = joint_control_mode;
ROS_ERROR("No %s for %s - We found %s",
ros_control_mode.c_str(),
joint_name.c_str(),
scontrol_mode.c_str());
return false;
}
if (scontrol_mode==sposition)
joint_control_mode=POSITION;
else if (scontrol_mode==svelocity)
joint_control_mode=VELOCITY;
else if (scontrol_mode==seffort)
joint_control_mode=EFFORT;
else {
ROS_ERROR("%s for %s not understood. Expected %s, %s or %s. Got %s",
ros_control_mode.c_str(),
joint_name.c_str(),
sposition.c_str(),
svelocity.c_str(),
seffort.c_str(),
scontrol_mode.c_str());
return false;
}
aJointSotHandle.ros_control_mode = joint_control_mode;
//aJointSotHandle.sot_control_mode = joint_control_mode;
return true;
}
......
......@@ -55,7 +55,10 @@ namespace sot_controller
{
lhi::JointHandle joint;
double desired_init_pose;
ControlMode sot_control_mode;
// This should not be handled in roscontrol_sot package. The control type
// should be handled in SoT directly, by externalizing the integration from
// the Device.
//ControlMode sot_control_mode;
ControlMode ros_control_mode;
};
......
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