Skip to content

Commit

Permalink
Increased buffer size for all topics. Finished keyboard op for elevat…
Browse files Browse the repository at this point in the history 
…or control
  • Loading branch information
@MohitShridhar
MohitShridhar committed Aug 19, 2014
1 parent 585e3e0 commit e866c78
Show file tree
Hide file tree
Showing 4 changed files with 60 additions and 26 deletions.
14 changes: 7 additions & 7 deletions src/controllers/dynamics_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,7 @@
/*
Limitations:
Topic messages are dropped sometimes while making the first service call
Sometimes service calls are dropped without notice. Solution: Implement 'wait for call'
*/

class DynamicsController
Expand Down Expand Up @@ -218,13 +218,13 @@ class DynamicsController

void setupControlTopics()
{
door_cmd_vel_pub = rosNode.advertise<geometry_msgs::Twist>("/door_controller/command", 10);
door_active_pub = rosNode.advertise<std_msgs::UInt32MultiArray>("/door_controller/active", 10);
door_cmd_vel_pub = rosNode.advertise<geometry_msgs::Twist>("/door_controller/command", 100);
door_active_pub = rosNode.advertise<std_msgs::UInt32MultiArray>("/door_controller/active", 1000);

elev_target_pub = rosNode.advertise<std_msgs::Int32>("/elevator_controller/target_floor", 10);
elev_active_pub = rosNode.advertise<std_msgs::UInt32MultiArray>("elevator_controller/active", 10);
elev_param_pub = rosNode.advertise<std_msgs::Float32MultiArray>("elevator_controller/param", 10);
elev_door_pub = rosNode.advertise<std_msgs::UInt8>("/elevator_controller/door", 10);
elev_target_pub = rosNode.advertise<std_msgs::Int32>("/elevator_controller/target_floor", 100);
elev_active_pub = rosNode.advertise<std_msgs::UInt32MultiArray>("elevator_controller/active", 1000);
elev_param_pub = rosNode.advertise<std_msgs::Float32MultiArray>("elevator_controller/param", 1000);
elev_door_pub = rosNode.advertise<std_msgs::UInt8>("/elevator_controller/door", 100);
}

std_msgs::UInt32MultiArray uintVectorToStdMsgs(std::vector<uint32_t> active_units)
Expand Down
56 changes: 45 additions & 11 deletions src/controllers/keyboard_op.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,7 @@ class KeyboardOp
dynamic_models::TargetFloorElev targetFloorCall;
dynamic_models::OpenCloseElevDoors openElevDoorsCall;
dynamic_models::OpenCloseElevDoors closeElevDoorsCall;
dynamic_models::SetElevProps setElevSpeedCall;
dynamic_models::SetElevProps setElevForceCall;
dynamic_models::SetElevProps setElevPropsCall;

public:
KeyboardOp(ros::NodeHandle &nh)
Expand Down Expand Up @@ -111,9 +110,25 @@ class KeyboardOp
addSrv.request.group.active_units = activeList;
add_group_client.call(addSrv);

type == DOOR ? printDoorControls() : printElevatorControls();

isGroupInitialized = true;
}

void printDoorControls()
{
std::cout << "\n-----------------\nDoor Controls:\nPress 'Enter' after each input.\n'q' to quit.\n'o' to open doors\n'c' to close doors";
std::cout << "\n'l' and 'value' to specify the linear velocity of a sliding door in m/s (eg 'l 10' or 'l-3.1')\n'a' and 'value' to specify angular velocity of a revolving door in radians (eg: 'a -1.57' or 'a3.14')";
std::cout << "\n'0' to stop movement\n-----------------\n" << std::endl;
}

void printElevatorControls()
{
std::cout << "\n-----------------\nElevator Controls:";
std::cout << "\nPress 'Enter' after each input.\n'q' to quit.\nEg: '4' goes to the fourth floor";
std::cout << "\n's##' to set the lift speed (eg: s4.2 for 4.2 m/s). Default: 1.5m/s\n'f##' to set the lift force (eg: f150 for 150N). Default: 150N\n'o' to force open the doors on the current floor\n'c' to force close the doors on the current floor\nDefault floor: 'F0'\n-----------------\n" << std::endl;
}

std::vector<uint32_t> parseActiveList(char input[])
{
std::string active_list_str(input);
Expand Down Expand Up @@ -178,13 +193,9 @@ class KeyboardOp
closeElevDoorsCall.request.group_name = CONTROL_GROUP_NAME;
closeElevDoorsCall.request.state = false;

setElevSpeedCall.request.group_name = CONTROL_GROUP_NAME;
setElevSpeedCall.request.velocity = DEFAULT_ELEV_SPEED;
setElevSpeedCall.request.force = DEFAULT_ELEV_FORCE;

setElevForceCall.request.group_name = CONTROL_GROUP_NAME;
setElevForceCall.request.velocity = DEFAULT_ELEV_SPEED;
setElevForceCall.request.force = DEFAULT_ELEV_FORCE;
setElevPropsCall.request.group_name = CONTROL_GROUP_NAME;
setElevPropsCall.request.velocity = DEFAULT_ELEV_SPEED;
setElevPropsCall.request.force = DEFAULT_ELEV_FORCE;
}

void readLineInput(char input[30])
Expand Down Expand Up @@ -230,7 +241,31 @@ class KeyboardOp

void executeElevatorServices(char input[])
{
std::string inputStr(input);

switch(input[0]) {
case 'o':
open_close_elev_doors_client.call(openElevDoorsCall);
break;
case 'c':
open_close_elev_doors_client.call(closeElevDoorsCall);
break;
case 's':
setElevPropsCall.request.velocity = parseFloat(inputStr.substr(1));
set_elev_props_client.call(setElevPropsCall);
break;
case 'f':
setElevPropsCall.request.force = parseFloat(inputStr.substr(1));
set_elev_props_client.call(setElevPropsCall);
break;
default:
try {
targetFloorCall.request.target_floor = std::stoi(inputStr);
target_floor_elev_client.call(targetFloorCall);
} catch(std::exception const & e) {
std::cout << "Unknown command" << std::endl;
}
};
}

void executeDoorServices(char input[])
Expand All @@ -254,8 +289,7 @@ class KeyboardOp
set_vel_doors_client.call(setVelDoorsCall);
break;
default:
std::cout << "Unknown command" << std::endl;

std::cout << "Unknown command" << std::endl;
};
}

Expand Down
8 changes: 4 additions & 4 deletions src/plugins/auto_elev_door_plugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,10 +144,10 @@ namespace gazebo
model = _parent;
doorLink = model->GetLink("door");

target_floor_sub = rosNode->subscribe<std_msgs::Int32>("/elevator_controller/target_floor", 5, &AutoElevDoorPlugin::target_floor_cb, this);
est_floor_sub = rosNode->subscribe<std_msgs::Int32>("/elevator_controller/" + elevator_ref_name + "/estimated_current_floor", 5, &AutoElevDoorPlugin::est_floor_cb, this);
open_close_sub = rosNode->subscribe<std_msgs::UInt8>("/elevator_controller/door", 5, &AutoElevDoorPlugin::open_close_cb, this);
active_elevs_sub = rosNode->subscribe<std_msgs::UInt32MultiArray>("/elevator_controller/active", 5, &AutoElevDoorPlugin::active_elevs_cb, this);
target_floor_sub = rosNode->subscribe<std_msgs::Int32>("/elevator_controller/target_floor", 50, &AutoElevDoorPlugin::target_floor_cb, this);
est_floor_sub = rosNode->subscribe<std_msgs::Int32>("/elevator_controller/" + elevator_ref_name + "/estimated_current_floor", 50, &AutoElevDoorPlugin::est_floor_cb, this);
open_close_sub = rosNode->subscribe<std_msgs::UInt8>("/elevator_controller/door", 50, &AutoElevDoorPlugin::open_close_cb, this);
active_elevs_sub = rosNode->subscribe<std_msgs::UInt32MultiArray>("/elevator_controller/active", 50, &AutoElevDoorPlugin::active_elevs_cb, this);

updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&AutoElevDoorPlugin::OnUpdate, this));
}
Expand Down
8 changes: 4 additions & 4 deletions src/plugins/elevator_plugin.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,10 +131,10 @@ namespace gazebo

rosNode = new ros::NodeHandle("");

target_floor_sub = rosNode->subscribe<std_msgs::Int32>("/elevator_controller/target_floor", 1, &ElevatorPlugin::target_floor_cb, this);
active_elevs_sub = rosNode->subscribe<std_msgs::UInt32MultiArray>("/elevator_controller/active", 5, &ElevatorPlugin::active_elevs_cb, this);
set_param_sub = rosNode->subscribe<std_msgs::Float32MultiArray>("/elevator_controller/param", 5, &ElevatorPlugin::set_param_cb, this);
estimated_floor_pub = rosNode->advertise<std_msgs::Int32>("/elevator_controller/" + modelName + "/estimated_current_floor", 10);
target_floor_sub = rosNode->subscribe<std_msgs::Int32>("/elevator_controller/target_floor", 100, &ElevatorPlugin::target_floor_cb, this);
active_elevs_sub = rosNode->subscribe<std_msgs::UInt32MultiArray>("/elevator_controller/active", 100, &ElevatorPlugin::active_elevs_cb, this);
set_param_sub = rosNode->subscribe<std_msgs::Float32MultiArray>("/elevator_controller/param", 100, &ElevatorPlugin::set_param_cb, this);
estimated_floor_pub = rosNode->advertise<std_msgs::Int32>("/elevator_controller/" + modelName + "/estimated_current_floor", 100);

updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ElevatorPlugin::OnUpdate, this));
}
Expand Down

0 comments on commit e866c78

Please sign in to comment.