Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
4 changes: 2 additions & 2 deletions sr_edc_ethercat_drivers/src/sr09.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -171,10 +171,10 @@ int SR09::initialize(hardware_interface::HardwareInterface *hw, bool allow_unpro

imu_gyr_scale_server_ = nodehandle_.advertiseService
<sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
("/" + imu_name + "/set_gyr_scale", boost::bind(&SR09::imu_scale_callback_, this, _1, _2, "gyr"));
("/" + imu_name + "/set_gyr_scale", [this](auto req, auto res){ return imu_scale_callback_(req, res, "gyr"); });
imu_acc_scale_server_ = nodehandle_.advertiseService
<sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
("/" + imu_name + "/set_acc_scale", boost::bind(&SR09::imu_scale_callback_, this, _1, _2, "acc"));
("/" + imu_name + "/set_acc_scale", [this](auto req, auto res){ return imu_scale_callback_(req, res, "acc"); });

ros::param::param<int>("/" + imu_name + "/acc_scale", imu_scale_acc_, 0);
ros::param::param<int>("/" + imu_name + "/gyr_scale", imu_scale_gyr_, 0);
Expand Down
4 changes: 2 additions & 2 deletions sr_edc_ethercat_drivers/src/sr10.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -183,10 +183,10 @@ int SR10::initialize(hardware_interface::HardwareInterface *hw, bool allow_unpro
// Broadcast /set_gyr_scale and /set_acc_scale services
imu_gyr_scale_server_ = nodehandle_.advertiseService
<sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
("/" + imu_name + "/set_gyr_scale", boost::bind(&SR10::imu_scale_callback_, this, _1, _2, "gyr"));
("/" + imu_name + "/set_gyr_scale", [this](auto req, auto res){ return imu_scale_callback_(req, res, "gyr"); });
imu_acc_scale_server_ = nodehandle_.advertiseService
<sr_robot_msgs::SetImuScale::Request, sr_robot_msgs::SetImuScale::Response>
("/" + imu_name + "/set_acc_scale", boost::bind(&SR10::imu_scale_callback_, this, _1, _2, "acc"));
("/" + imu_name + "/set_acc_scale", [this](auto req, auto res){ return imu_scale_callback_(req, res, "acc"); });

// Initialize default IMU scaling values
ros::param::param<int>("/" + imu_name + "/acc_scale", imu_scale_acc_, 0);
Expand Down
3 changes: 2 additions & 1 deletion sr_robot_lib/src/generic_updater.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,10 +46,11 @@ namespace generic_updater
double tmp_dur = config.when_to_update;
ros::Duration duration(tmp_dur);

int32u data_type = config.what_to_update;
timers.push_back(
nh_tilde.createTimer(
duration,
boost::bind(&GenericUpdater::timer_callback, this, _1, config.what_to_update)));
[this, data_type](auto event){ timer_callback(event, data_type); }));
}
else
{
Expand Down
4 changes: 2 additions & 2 deletions sr_robot_lib/src/motor_data_checker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@
* @author toni <toni@shadowrobot.com>
* @date 25 Oct 2011
*
/* Copyright 2011 Shadow Robot Company Ltd.
/* Copyright 2011, 2024 Shadow Robot Company Ltd.
*
* This program is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
Expand Down Expand Up @@ -53,7 +53,7 @@ namespace generic_updater
{
// Create a one-shot timer
check_timeout_timer = nh_tilde.createTimer(init_max_duration,
boost::bind(&MotorDataChecker::timer_callback, this, _1), true);
[this](auto event){ timer_callback(event); }, true);
update_state = operation_mode::device_update_state::INITIALIZATION;
msg_checkers_.clear();

Expand Down
16 changes: 7 additions & 9 deletions sr_robot_lib/src/sr_motor_hand_lib.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -164,25 +164,23 @@ namespace shadow_robot
ostringstream ss;
ss << "change_force_PID_" << joint_names[index];
// initialize the force pid service
const int motor_id = motor_wrapper->motor_id;
auto change_force_cb = [this, motor_id](auto req, auto res){ return force_pid_callback(req, res, motor_id); };
// NOTE: the template keyword is needed to avoid a compiler complaint apparently due to the fact that
// we are using an explicit template function inside this template class
motor_wrapper->force_pid_service =
this->nh_tilde.template advertiseService<sr_robot_msgs::ForceController::Request,
sr_robot_msgs::ForceController::Response>(ss.str().c_str(),
boost::bind(
&SrMotorHandLib<StatusType,
CommandType>::force_pid_callback,
this, _1, _2,
motor_wrapper->motor_id));
sr_robot_msgs::ForceController::Response>(ss.str().c_str(), change_force_cb);

ss.str("");
ss << "reset_motor_" << joint_names[index];
// initialize the reset motor service
pair<int, string> joint_pair(motor_wrapper->motor_id, joint.joint_name);
auto reset_motor_cb =
[this, joint_pair](auto req, auto res){ return reset_motor_callback(req, res, joint_pair); };
motor_wrapper->reset_motor_service =
this->nh_tilde.template advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>(
ss.str().c_str(),
boost::bind(&SrMotorHandLib<StatusType, CommandType>::reset_motor_callback, this, _1, _2,
pair<int, string>(motor_wrapper->motor_id, joint.joint_name)));
ss.str().c_str(), reset_motor_cb);
}
else
{ // no motor associated to this joint
Expand Down
9 changes: 3 additions & 6 deletions sr_robot_lib/src/sr_muscle_hand_lib.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
* @date Tue Mar 19 17:12:13 2013
*
*
/* Copyright 2013 Shadow Robot Company Ltd.
/* Copyright 2013, 2024 Shadow Robot Company Ltd.
*
* This program is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
Expand Down Expand Up @@ -113,13 +113,10 @@ namespace shadow_robot
ostringstream ss;
ss << "reset_muscle_driver_" << i;
// initialize the reset muscle driver service
auto reset_driver_cb = [this, i](auto req, auto res){ return reset_muscle_driver_callback(req, res, i); };
driver.reset_driver_service =
this->nh_tilde.template advertiseService<std_srvs::Empty::Request,
std_srvs::Empty::Response>(ss.str().c_str(),
boost::bind(
&SrMuscleHandLib<StatusType,
CommandType>::reset_muscle_driver_callback,
this, _1, _2, i));
std_srvs::Empty::Response>(ss.str().c_str(), reset_driver_cb);

this->muscle_drivers_vector_.push_back(driver);
}
Expand Down
7 changes: 2 additions & 5 deletions sr_robot_lib/src/sr_muscle_robot_lib.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@
* @date Tue Mar 19 17:12:13 2013
*
*
/* Copyright 2013 Shadow Robot Company Ltd.
/* Copyright 2013, 2024 Shadow Robot Company Ltd.
*
* This program is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the Free
Expand Down Expand Up @@ -75,10 +75,7 @@ namespace shadow_robot

// Create a one-shot timer
check_init_timeout_timer(this->nh_tilde.createTimer(init_max_duration,
boost::bind(
&SrMuscleRobotLib<StatusType,
CommandType>::init_timer_callback,
this, _1), true)),
[this](auto ev){ init_timer_callback(ev); }, true)),
pressure_calibration_map_(read_pressure_calibration())
{
#ifdef DEBUG_PUBLISHER
Expand Down
5 changes: 1 addition & 4 deletions sr_robot_lib/src/sr_robot_lib.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -220,10 +220,7 @@ namespace shadow_robot
// Create a one-shot timer
tactile_check_init_timeout_timer(
this->nh_tilde.createTimer(tactile_init_max_duration,
boost::bind(
&SrRobotLib<StatusType,
CommandType>::tactile_init_timer_callback,
this, _1), true)),
[this](auto event){ tactile_init_timer_callback(event); }, true)),
lock_tactile_init_timeout_(boost::shared_ptr<boost::mutex>(new boost::mutex())),
tactiles_init(shared_ptr<GenericTactiles<StatusType, CommandType> >(
new GenericTactiles<StatusType, CommandType>(nodehandle_, device_id_,
Expand Down