Skip to content
Snippets Groups Projects
Commit 32c933b8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added a service to enable/disable the servos power.

parent 990c40b0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
cfg/ArmDriver.cfg
+ 7
− 7
View file @ 32c933b8
......@@ -54,7 +54,7 @@ gen.add("serial_device", str_t, SensorLevels.RECONFIGURE_CLOSE, "
gen.add("baudrate", int_t, SensorLevels.RECONFIGURE_CLOSE, "Baudrate of the serial device",1000000,9600,1000000)
gen.add("positions_file", str_t, SensorLevels.RECONFIGURE_RUNNING, "Name of he file with the stored position", "")
gen.add("save_positions", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Save current position", False)
gen.add("server_ip_addr", str_t, SensorLevels.RECONFIGURE_STOP, "Server IP address", "192.168.8.117")
gen.add("server_ip_addr", str_t, SensorLevels.RECONFIGURE_STOP, "Server IP address", "31.4.247.101")
gen.add("server_port", int_t, SensorLevels.RECONFIGURE_STOP, "Server port", 1234, 1025, 65535)
gen.add("max_speed", int_t, SensorLevels.RECONFIGURE_RUNNING, "Maximum speed for the pre-defined positions",50, 10, 200)
imu.add("imu_control_mode",int_t, SensorLevels.RECONFIGURE_RUNNING, "IMU control mode", 1, 1, 2, edit_method=enum_imu_control_mode)
......@@ -65,18 +65,18 @@ gui.add("gui_pan_dead_zone",double_t,SensorLevels.RECONFIGURE_RUNNING, "
gui.add("gui_tilt_gain", double_t, SensorLevels.RECONFIGURE_RUNNING, "Tilt gain when using the GUI",0.5,0.01,10.0)
gui.add("gui_tilt_dead_zone",double_t,SensorLevels.RECONFIGURE_RUNNING, "Tilt dead zone when using the GUI",0.1,0.0,1.0)
pan.add("pan_id", int_t, SensorLevels.RECONFIGURE_CLOSE, "ID of the pan servo", 4, 0, 253)
pan.add("max_pan_angle", double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. angle for the pan servo (degrees)",80.0,0.0,150.0)
pan.add("min_pan_angle", double_t, SensorLevels.RECONFIGURE_RUNNING, "Min. angle for the pan servo (degrees)",-80.0,-150.0,0.0)
pan.add("max_pan_angle", double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. angle for the pan servo (degrees)",80.0,-150.0,150.0)
pan.add("min_pan_angle", double_t, SensorLevels.RECONFIGURE_RUNNING, "Min. angle for the pan servo (degrees)",-80.0,-150.0,150.0)
pan.add("inv_pan_dir", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Invert pan positive direction",False)
pan.add("max_pan_torque", double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. torque for the pan servo (%)",50.0,0.0,100.0)
tilt.add("tilt_id", int_t, SensorLevels.RECONFIGURE_CLOSE, "ID of the tilt servo", 5, 0, 253)
tilt.add("max_tilt_angle",double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. angle for the tilt servo (degrees)",80.0,0.0,150.0)
tilt.add("min_tilt_angle",double_t, SensorLevels.RECONFIGURE_RUNNING, "Min. angle for the tilt servo (degrees)",-80.0,-150.0,0.0)
tilt.add("max_tilt_angle",double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. angle for the tilt servo (degrees)",80.0,-150.0,150.0)
tilt.add("min_tilt_angle",double_t, SensorLevels.RECONFIGURE_RUNNING, "Min. angle for the tilt servo (degrees)",-80.0,-150.0,150.0)
tilt.add("inv_tilt_dir", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Invert tilt positive direction",False)
tilt.add("max_tilt_torque",double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. torque for the tilt servo (%)",50.0,0.0,100.0)
roll.add("roll_id", int_t, SensorLevels.RECONFIGURE_CLOSE, "ID of the roll servo", 6, 0, 253)
roll.add("max_roll_angle",double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. angle for the roll servo (degrees)",80.0,0.0,150.0)
roll.add("min_roll_angle",double_t, SensorLevels.RECONFIGURE_RUNNING, "Min. angle for the roll servo (degrees)",-80.0,-150.0,0.0)
roll.add("max_roll_angle",double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. angle for the roll servo (degrees)",80.0,-150.0,150.0)
roll.add("min_roll_angle",double_t, SensorLevels.RECONFIGURE_RUNNING, "Min. angle for the roll servo (degrees)",-80.0,-150.0,150.0)
roll.add("inv_roll_dir", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Invert roll positive direction",False)
roll.add("max_roll_torque",double_t, SensorLevels.RECONFIGURE_RUNNING, "Max. torque for the roll servo (%)",50.0,0.0,100.0)
......
config/arm2_params.yaml
+ 8
− 8
View file @ 32c933b8
......@@ -4,26 +4,26 @@ baudrate: 115200
pan_id: 20
tilt_id: 6
roll_id: 4
server_ip_addr: 192.168.8.117
server_ip_addr: 31.4.247.101
server_port: 1234
max_speed: 50
max_speed: 100
imu_control_mode: 1
imu_threshold: 3
imu_gain: 0.5
gui_pan_gain: 0.1
gui_pan_gain: 0.05
gui_pan_dead_zone: 0.1
gui_tilt_gain: 0.1
gui_tilt_gain: 0.05
gui_tilt_dead_zone: 0.1
max_pan_angle: 90.0
min_pan_angle: -90.0
inv_pan_dir: True
max_pan_torque: 50.0
max_tilt_angle: 45.0
min_tilt_angle: -45.0
max_tilt_angle: 90.0
min_tilt_angle: 35.0
inv_tilt_dir: True
max_tilt_torque: 50.0
max_roll_angle: 10.0
min_roll_angle: -10.0
max_roll_angle: 100.0
min_roll_angle: 80.0
inv_roll_dir: False
max_roll_torque: 50.0
config/arm2_poses.xml
+ 8
− 8
View file @ 32c933b8
......@@ -3,25 +3,25 @@
<arm_motion>
<button_name>triangle</button_name>
<pan>0.0</pan>
<tilt>0.0</tilt>
<roll>0.0</roll>
<tilt>45.0</tilt>
<roll>90.0</roll>
</arm_motion>
<arm_motion>
<button_name>circle</button_name>
<pan>-90.0</pan>
<tilt>0.0</tilt>
<roll>0.0</roll>
<tilt>45.0</tilt>
<roll>90.0</roll>
</arm_motion>
<arm_motion>
<button_name>cross</button_name>
<pan>90.0</pan>
<tilt>0.0</tilt>
<roll>0.0</roll>
<tilt>45.0</tilt>
<roll>90.0</roll>
</arm_motion>
<arm_motion>
<button_name>square</button_name>
<pan>0.0</pan>
<tilt>45.0</tilt>
<roll>0.0</roll>
<tilt>90.0</tilt>
<roll>90.0</roll>
</arm_motion>
</arm_motions>
include/arm_driver_driver.h
+ 3
− 0
View file @ 32c933b8
......@@ -161,6 +161,9 @@ class ArmDriver : public iri_base_driver::IriBaseDriver
void move(unsigned int axis_id,double torque);
void stop(unsigned int axis_id=ALL);
void get_current_angles(double &pan, double &tilt,double &roll);
void enable_servo_power(void);
void disable_servo_power(void);
bool is_servo_power_enabled(void);
// stored poses
void increase_max_speed(void);
void decrease_max_speed(void);
......
include/arm_driver_driver_node.h
+ 6
− 0
View file @ 32c933b8
......@@ -34,6 +34,7 @@
#include <sensor_msgs/Imu.h>
// [service client headers]
#include <std_srvs/SetBool.h>
// [action server client headers]
#include "socketudp.h"
......@@ -97,6 +98,11 @@ class ArmDriverNode : public iri_base_driver::IriBaseNodeDriver<ArmDriver>
TGUIInfo gui_info;
// [service attributes]
ros::ServiceServer power_servos_server_;
bool power_servosCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
pthread_mutex_t power_servos_mutex_;
void power_servos_mutex_enter(void);
void power_servos_mutex_exit(void);
// [client attributes]
......
src/arm_driver_driver.cpp
+ 21
− 0
View file @ 32c933b8
......@@ -22,6 +22,7 @@ bool ArmDriver::openDriver(void)
this->arm->set_max_angles(ROLL,this->config_.max_roll_angle,this->config_.min_roll_angle);
this->arm->invert_direction(ROLL,this->config_.inv_roll_dir);
this->arm->set_max_torque(ROLL,this->config_.max_roll_torque);
this->arm->set_max_speed(this->config_.max_speed);
return true;
}catch(CException &e){
......@@ -265,6 +266,26 @@ void ArmDriver::get_current_angles(double &pan, double &tilt,double &roll)
}
}
void ArmDriver::enable_servo_power(void)
{
if(this->arm!=NULL)
this->arm->enable_servo_power();
}
void ArmDriver::disable_servo_power(void)
{
if(this->arm!=NULL)
this->arm->disable_servo_power();
}
bool ArmDriver::is_servo_power_enabled(void)
{
if(this->arm!=NULL)
return this->arm->is_servo_power_enabled();
else
return false;
}
// stored poses
void ArmDriver::increase_max_speed(void)
{
......
src/arm_driver_driver_node.cpp
+ 34
− 1
View file @ 32c933b8
......@@ -61,6 +61,8 @@ ArmDriverNode::ArmDriverNode(ros::NodeHandle &nh) :
this->setup_socket(this->config_.server_ip_addr,this->config_.server_port);
// [init services]
this->power_servos_server_ = this->private_node_handle_.advertiseService("power_servos", &ArmDriverNode::power_servosCallback, this);
pthread_mutex_init(&this->power_servos_mutex_,NULL);
// [init clients]
......@@ -114,7 +116,6 @@ void ArmDriverNode::mainNodeThread(void)
}
if(this->gui_info.move_active)
{
ROS_WARN_STREAM("move: " << this->gui_info.x_coord << "," << this->gui_info.y_coord);
if(this->driver_.is_pose_running())
this->driver_.stop_pose();
if(!this->driver_.is_enabled(PAN))
......@@ -287,6 +288,38 @@ void ArmDriverNode::imu_mutex_exit(void)
/* [service callbacks] */
bool ArmDriverNode::power_servosCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
ROS_INFO("ArmDriverNode::power_servosCallback: New Request Received!");
//use appropiate mutex to shared variables if necessary
this->driver_.lock();
//this->power_servos_mutex_enter();
//ROS_INFO("ArmDriverNode::power_servosCallback: Processing New Request!");
//do operations with req and output on res
if(req.data)
this->driver_.enable_servo_power();
else
this->driver_.disable_servo_power();
//res.data2 = req.data1 + my_var;
//unlock previously blocked shared variables
//this->power_servos_mutex_exit();
this->driver_.unlock();
return true;
}
void ArmDriverNode::power_servos_mutex_enter(void)
{
pthread_mutex_lock(&this->power_servos_mutex_);
}
void ArmDriverNode::power_servos_mutex_exit(void)
{
pthread_mutex_unlock(&this->power_servos_mutex_);
}
/* [action callbacks] */
......
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