Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Speed control added #45

Open
wants to merge 1 commit into
base: indigo-devel
Choose a base branch
from
Open

Speed control added #45

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions CHANGELOG.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,11 @@
Changelog for package teleop_twist_joy
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

0.1.4 (2023-07-17)
------------------
* Added max speed control
* Contributors: Hasan Ozcan

0.1.3 (2019-01-21)
------------------
* Use industrial ci
Expand Down
98 changes: 98 additions & 0 deletions src/teleop_twist_joy.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,12 +43,21 @@ struct TeleopTwistJoy::Impl
{
void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
void sendCmdVelMsg(const sensor_msgs::Joy::ConstPtr& joy_msg, const std::string& which_map);
void speed_control(const sensor_msgs::Joy::ConstPtr& joy_msg);

ros::Subscriber joy_sub;
ros::Publisher cmd_vel_pub;

int enable_button;
int enable_turbo_button;
int linear_increase_button;
int linear_decrease_button;
int angular_increase_button;
int angular_decrease_button;
double increase_value;
double decrease_value;
// These variables will be used to control only when the button is pressed.
bool speed_control_state[3] = {false,false,false};

std::map<std::string, int> axis_linear_map;
std::map< std::string, std::map<std::string, double> > scale_linear_map;
Expand All @@ -73,6 +82,26 @@ TeleopTwistJoy::TeleopTwistJoy(ros::NodeHandle* nh, ros::NodeHandle* nh_param)

nh_param->param<int>("enable_button", pimpl_->enable_button, 0);
nh_param->param<int>("enable_turbo_button", pimpl_->enable_turbo_button, -1);
nh_param->param<int>("linear_increase_button", pimpl_->linear_increase_button, -1);
nh_param->param<int>("linear_decrease_button", pimpl_->linear_decrease_button, -1);
nh_param->param<int>("angular_increase_button", pimpl_->angular_increase_button, -1);
nh_param->param<int>("angular_decrease_button", pimpl_->angular_decrease_button, -1);
nh_param->param<double>("increase_value", pimpl_->increase_value, 0.01);
nh_param->param<double>("decrease_value", pimpl_->decrease_value, 0.01);

if(pimpl_->increase_value<0.0)
{
ROS_ERROR("İncrease value cannot be negative");
pimpl_->increase_value*=-1;
ROS_WARN("Increment value set to %f",pimpl_->increase_value);
}
if(pimpl_->decrease_value<0.0)
{
ROS_ERROR("Decrease value cannot be negative");
pimpl_->decrease_value*=-1;
ROS_WARN("Increment value set to %f",pimpl_->decrease_value);
}


if (nh_param->getParam("axis_linear", pimpl_->axis_linear_map))
{
Expand Down Expand Up @@ -154,8 +183,77 @@ void TeleopTwistJoy::Impl::sendCmdVelMsg(const sensor_msgs::Joy::ConstPtr& joy_m
sent_disable_msg = false;
}

void TeleopTwistJoy::Impl::speed_control(const sensor_msgs::Joy::ConstPtr& joy_msg)
{
if(linear_increase_button>0 && joy_msg->buttons[linear_increase_button] )
{
if(!speed_control_state[0])
{
speed_control_state[0]=true;
if( scale_linear_map["normal"]["x"]< scale_linear_map["turbo"]["x"])
{
scale_linear_map["normal"]["x"] += increase_value;
}
ROS_INFO("linear max speed %f",scale_linear_map["normal"]["x"]);
}
}else{
speed_control_state[0]=false;
}

if(linear_decrease_button>0 && joy_msg->buttons[linear_decrease_button] )
{
if(!speed_control_state[1])
{
speed_control_state[1]=true;

scale_linear_map["normal"]["x"] -= decrease_value;
if(scale_linear_map["normal"]["x"]<0.01)
{
scale_linear_map["normal"]["x"] =0.01;
}
ROS_INFO("Linear max speed %f",scale_linear_map["normal"]["x"]);
}
}else{
speed_control_state[1]=false;
}

if(angular_increase_button>0 && joy_msg->buttons[angular_increase_button] )
{
if(!speed_control_state[2])
{
speed_control_state[2]=true;
if( scale_angular_map["normal"]["yaw"]< scale_angular_map["turbo"]["yaw"])
{
scale_angular_map["normal"]["yaw"] += increase_value;
}
ROS_INFO("Angular max speed %f",scale_angular_map["normal"]["yaw"]);
}
}else{
speed_control_state[2]=false;
}

if(linear_increase_button>0 && joy_msg->buttons[angular_decrease_button] )
{
if(!speed_control_state[3])
{
speed_control_state[3]=true;
scale_angular_map["normal"]["yaw"] -= decrease_value;
if(scale_angular_map["normal"]["yaw"]<0.01)
{
scale_angular_map["normal"]["yaw"] =0.01;
}
ROS_INFO("Angular max speed %f",scale_angular_map["normal"]["yaw"]);
}
}else{
speed_control_state[3]=false;
}
}

void TeleopTwistJoy::Impl::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg)
{

speed_control(joy_msg);

if (enable_turbo_button >= 0 &&
joy_msg->buttons.size() > enable_turbo_button &&
joy_msg->buttons[enable_turbo_button])
Expand Down