int main(int argc, char **argv){
ros::init(argc, argv, "mobots_teleop_bridge");
ros::NodeHandle n;
client = n.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state"); //true für persistente Verbindung <> GEHT NICHT IN DIESEM FALL >_<
client.waitForExistence();
getClient = n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state", true);
getClient.waitForExistence();
ros::Subscriber sub = n.subscribe("/cmd_vel", 100, teleopCallback); //wichtig: Wenn das zurückgegebene Objekt nicht mehr referenziert wird, wird der tatsächliche Subscriber zerstört
ros::spin();
}
PickPlaceDemo() :
arm("arm"),
gripper("gripper")
{
planning_scene_diff_client = node_handle.serviceClient<moveit_msgs::ApplyPlanningScene>("apply_planning_scene");
planning_scene_diff_client.waitForExistence();
get_planning_scene_client = node_handle.serviceClient<moveit_msgs::GetPlanningScene>("get_planning_scene");
get_planning_scene_client.waitForExistence();
}
bool wait_for_services() {
state_machine_client.waitForExistence();
ROS_INFO("State machine service is ready, waiting for init");
hdpc_com::ChangeStateMachine change_state;
ros::Rate rate(1);
do {
rate.sleep();
change_state.request.event = EVENT_READ_STATE;
if (!state_machine_client.call(change_state)) {
ROS_WARN("Change state machine: request state failed");
return false;
}
} while (ros::ok() && (change_state.response.state != SM_STOP));
ROS_INFO("HDPC Drive: ROVER is ready");
change_state.request.event = EVENT_START;
if (!state_machine_client.call(change_state)) {
ROS_WARN("Change state machine: starting failed");
return false;
}
watchdog = 0;
control_mode = HDPCModes::MODE_INIT;
commands.header.stamp = ros::Time::now();
for (unsigned int i=0;i<10;i++) {
commands.isActive[i] = false;
commands.velocity[i] = 0;
commands.position[i] = 0.0;
}
return true;
}
int
main (int argc, char** argv)
{
ros::init (argc, argv, "al_viz_objects_features");
nh_ = new ros::NodeHandle ("~");
sub_entities_ = nh_->subscribe ("/perception/entities", 1, &entitiesCallback);
srv_cl_get_entities_visual_features_
= nh_->serviceClient<al_srvs::GetEntitiesVisualFeatures> ("/get_entities_visual_features", true);
srv_cl_get_entities_visual_features_.waitForExistence ();
srv_cl_get_entities_visual_features_
= nh_->serviceClient<al_srvs::GetEntitiesVisualFeatures> ("/get_entities_visual_features", true);
ep_ = engOpen ("matlab -nodesktop -nosplash -nojvm");
if (ep_ == NULL)
ROS_ERROR("Constructor: engOpen failed");
else
ROS_DEBUG(" Matlab engine is working!");
//in any case surface normals should be calculated
pcl::PointCloud<pcl::Normal>::Ptr pointcloud_normals (new pcl::PointCloud<pcl::Normal>);
pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
ne_.setSearchMethod (tree);
ne_.setViewPoint (0, 0, 1.2);//this could be done more informed by getting this information from a topic
ros::Rate r (30);//objects can be refreshed at most @30hz
while (nh_->ok ())
{
if (msg_entities_rcvd_)
{
msg_entities_rcvd_ = false;
vizFeatures ();
}
ros::spinOnce ();
r.sleep ();
}
engClose (ep_);
return 0;
}
// Close end effector
bool closeEndEffector()
{
// Error check - servos are alive and we've been recieving messages
if( !endEffectorResponding() )
{
return false;
}
// Check if end effector is already close and arm is stil
if( ee_status_.target_position == END_EFFECTOR_CLOSE_VALUE_MAX &&
ee_status_.moving == false &&
ee_status_.position > END_EFFECTOR_CLOSE_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_CLOSE_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE )
{
// Consider the ee to already be in the correct position
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector already closed within tolerance, unable to close further");
return true;
}
// Set the velocity for the end effector to a low value
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity low");
velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME);
if( !velocity_client_.waitForExistence(ros::Duration(5.0)) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Timed out waiting for velocity client existance");
return false;
}
dynamixel_hardware_interface::SetVelocity set_velocity_srv;
set_velocity_srv.request.velocity = END_EFFECTOR_MEDIUM_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
std_msgs::Float64 joint_value;
double timeout_sec = 10.0; // time before we declare an error
const double CHECK_INTERVAL = 0.1; // how often we check the load
const double FIRST_BACKOUT_AMOUNT = -0.25;
const double SECOND_BACKOUT_AMOUNT = -0.0075;
const double BACKOUT_AMOUNT[2] = {FIRST_BACKOUT_AMOUNT, SECOND_BACKOUT_AMOUNT};
// Grasp twice - to reduce amount of slips and ensure better grip
for(int i = 0; i < 2; ++i)
{
timeout_sec = 10; // reset timeout;
ROS_DEBUG_STREAM("Grasping with end effector - grasp number " << i + 1);
// Tell servos to start closing slowly to max amount
joint_value.data = END_EFFECTOR_CLOSE_VALUE_MAX;
end_effector_pub_.publish(joint_value);
// Wait until end effector is done moving
while( ee_status_.position < joint_value.data - END_EFFECTOR_POSITION_TOLERANCE ||
ee_status_.position > joint_value.data + END_EFFECTOR_POSITION_TOLERANCE )
{
ros::spinOnce(); // Allows ros to get the latest servo message - we need the load
// Check if load has peaked
if( ee_status_.load < END_EFFECTOR_LOAD_SETPOINT ) // we have touched object!
{
// Open the gripper back up a little to reduce the amount of load.
// the first time open it a lot to help with grasp quality
joint_value.data = ee_status_.position + BACKOUT_AMOUNT[i];
// Check that we haven't passed the open limit
if( joint_value.data < END_EFFECTOR_OPEN_VALUE_MAX )
joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;
ROS_DEBUG_NAMED("clam_gripper_controller","Setting end effector setpoint to %f when it was %f", joint_value.data, ee_status_.position);
end_effector_pub_.publish(joint_value);
if( i == 0 ) // give lots of time to pull out the first time
{
ros::Duration(1.00).sleep();
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Sleeping as we publish joint value " << joint_value.data);
set_velocity_srv.request.velocity = END_EFFECTOR_SLOW_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
ros::Duration(1.0).sleep();
}
break;
}
// Debug output
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","" << joint_value.data - END_EFFECTOR_POSITION_TOLERANCE << " < " <<
ee_status_.position << " < " << joint_value.data + END_EFFECTOR_POSITION_TOLERANCE
<< " -- LOAD: " << ee_status_.load );
// Feedback
feedback_.position = ee_status_.position;
//TODO: fill in more of the feedback
action_server_->publishFeedback(feedback_);
// Wait an interval before checking again
ros::Duration(CHECK_INTERVAL).sleep();
// Check if timeout has occured
timeout_sec -= CHECK_INTERVAL;
if( timeout_sec <= 0 )
{
ROS_ERROR_NAMED("clam_gripper_controller","Timeout: Unable to close end effector");
return false;
}
}
}
// DONE
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished closing end effector action");
return true;
}
// Open end effector
bool openEndEffector()
{
// Error check - servos are alive and we've been recieving messages
if( !endEffectorResponding() )
{
return false;
}
// Check if end effector is already open and arm is still
if( ee_status_.target_position == END_EFFECTOR_OPEN_VALUE_MAX &&
ee_status_.moving == false &&
ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE )
{
// Consider the ee to already be in the corret position
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","End effector open: already in position");
return true;
}
// Set the velocity for the end effector servo
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Setting end effector servo velocity");
velocity_client_ = nh_.serviceClient< dynamixel_hardware_interface::SetVelocity >(EE_VELOCITY_SRV_NAME);
while(!velocity_client_.waitForExistence(ros::Duration(10.0)))
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
dynamixel_hardware_interface::SetVelocity set_velocity_srv;
set_velocity_srv.request.velocity = END_EFFECTOR_VELOCITY;
if( !velocity_client_.call(set_velocity_srv) )
{
ROS_ERROR_STREAM_NAMED("clam_gripper_controller","Failed to set the end effector servo velocity via service call");
return false;
}
// Publish command to servos
std_msgs::Float64 joint_value;
joint_value.data = END_EFFECTOR_OPEN_VALUE_MAX;
end_effector_pub_.publish(joint_value);
// Wait until end effector is done moving
int timeout = 0;
while( ee_status_.moving == true &&
ee_status_.position > END_EFFECTOR_OPEN_VALUE_MAX + END_EFFECTOR_POSITION_TOLERANCE &&
ee_status_.position < END_EFFECTOR_OPEN_VALUE_MAX - END_EFFECTOR_POSITION_TOLERANCE &&
ros::ok() )
{
// Feedback
feedback_.position = ee_status_.position;
//TODO: fill in more of the feedback
action_server_->publishFeedback(feedback_);
// Looping
ros::Duration(0.25).sleep();
++timeout;
if( timeout > 16 ) // wait 4 seconds
{
ROS_ERROR_NAMED("clam_gripper_controller","Unable to open end effector: timeout on goal position");
return false;
}
}
// It worked!
ROS_DEBUG_STREAM_NAMED("clam_gripper_controller","Finished end effector action");
return true;
}
int
main (int argc, char** argv)
{
ros::init (argc, argv, "al_object_affordances");
nh_ = new ros::NodeHandle ();
// ros::ServiceClient srv_cl_get_entity_;
// ros::ServiceClient srv_cl_get_entity_features_;
pub_affordances_ = nh_->advertise<al_msgs::Affordances> ("/affordances", 1);
srv_cl_affordances_ = nh_->serviceClient<al_srvs::GetLearnedEffects> ("/get_learned_effects", true);
while (nh_->ok () && !srv_cl_affordances_.waitForExistence (ros::Duration (0.1)))
{
ros::spinOnce ();
}
// srv_cl_get_entity_ = nh_->serviceClient<al_srvs::GetEntity> ("/memory/get_entity", true);
// while (nh_->ok () && !srv_cl_get_entity_.exists ())
// {
// srv_cl_get_entity_.waitForExistence (ros::Duration (0.1));
// ros::spinOnce ();
// }
//
// srv_cl_get_entity_features_ = nh_->serviceClient<al_srvs::GetEntityVisualFeatures> ("/get_entity_visual_features",
// true);
// while (nh_->ok () && !srv_cl_get_entity_features_.exists ())
// {
// srv_cl_get_entity_features_.waitForExistence (ros::Duration (0.1));
// ros::spinOnce ();
// }
al::system::changeInputMode (1);
getAllScene (entities_);
// ros::Rate r (5);
// std::cout << "Enter an object id to track its affordances" << std::endl;
// int id = -1;
// while (nh_->ok ())
// {
// char c_id;
// if (al::system::getKey (c_id))
// {
// std::cin >> c_id;
// id = atoi (&c_id);
// std::cout << "Enter an object id to track its affordances" << std::endl;
// }
//
// if (id != -1)
// {
// //get object
// al_srvs::GetEntity::Request req_entity;
// al_srvs::GetEntity::Response res_entity;
// req_entity.id = al::facilities::toString (id);
// req_entity.latest_scene_required = true;
// if (srv_cl_get_entity_.call (req_entity, res_entity))
// {
// //get object features
// al_srvs::GetEntityVisualFeatures::Request req_features;
// al_srvs::GetEntityVisualFeatures::Response res_features;
//
// req_features.entity = res_entity.entity;
// if (srv_cl_get_entity_features_.call (req_features, res_features))
// {
// al_srvs::GetLearnedEffects::Request req_learned_effects;
// al_srvs::GetLearnedEffects::Response res_learned_effects;
//
// req_learned_effects.feature_vector = res_features.feature_vector;
// if (srv_cl_affordances_.call (req_learned_effects, res_learned_effects))
// {
// for (uint i = 0; i < res_learned_effects.effects.size (); i++)
// {
// std::cout << al::facilities::behaviorEnumToString (res_learned_effects.effects[i].arg) << "\t";
// std::cout << al::facilities::effectEnumToString (res_learned_effects.effects[i].effect) << "\t";
// std::cout << res_learned_effects.effects[i].prob << std::endl;
// // std::cout << res_learned_effects.effects[i] << std::endl;
// map_effects[res_learned_effects.effects[i].prob] = res_learned_effects.effects[i];
// }
//
// //now pick top 5 different affordances
// //convert into an affordance message and publish
// it = map_effects.end ();
// // affordances_.effects.resize (TOP_N_EFFECTS);
// while (it != map_effects.begin () && affordances_.effects.size () < TOP_N_EFFECTS)
// {
// --it;
// bool same_effect_exists = false;
// for (uint i = 0; i < affordances_.effects.size (); i++)
// {
// if (it->second.effect == affordances_.effects[i].effect)
// {
// same_effect_exists = true;
// if (it->first > affordances_.effects[i].prob)
// {
// affordances_.effects[i].effect = it->second.effect;
// break;
// }
// }
// }
// if (!same_effect_exists)
// {
// affordances_.effects.push_back (it->second);
// }
// }
//
// affordances_.object = res_entity.entity.collision_object;
// std::cout << "n_affordances: " << (int)affordances_.effects.size () << std::endl;
// pub_affordances_.publish (affordances_);
// map_effects.clear ();
// affordances_.effects.clear ();
// }
// }
// }
// }
// ros::spinOnce ();
// r.sleep ();
// }
al::system::changeInputMode (0);
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pendulum_controller");
//ros::init(argc, argv, "pendulum");
ros::NodeHandle n("pendulum");
ros::NodeHandle n_state("pendulum");
command_pub = n.advertise<pendulum::command>( "command", 1000 );
state_client = n_state.serviceClient<pendulum::state>( "state" );
//state_client = n.serviceClient<pendulum::state>( "state" );
//state_client = n.serviceClient<pendulum::state>( "state", true );
if( !state_client.isValid( ) ) {
ROS_INFO( "state_client not valid" );
state_client.waitForExistence( );
}
ros::Rate loop_rate( 25 );
//ros::Rate loop_rate( 10 );
ROS_INFO("Ready to standup pendulum.");
command.time = 0.0;
command.torque = 0.0;
state.request.timestamp = 0.0;
pendulum::state s;
if( !state_client.isValid( ) ) {
ROS_INFO( "state_client no longer valid" );
state_client.waitForExistence( );
}
int count = 0;
ros::Time calibrate_time, start_time;
while( ros::ok( ) ) {
calibrate_time = ros::Time::now( );
if( calibrate_time.sec != 0 || calibrate_time.nsec != 0 )
break;
ros::spinOnce( );
}
start_time = ros::Time::now( );
//ROS_INFO( "start_time: sec[%u] nsec[%u]", start_time.sec, start_time.nsec );
ROS_INFO( "time, position, velocity, torque" );
while( ros::ok( ) ) {
//if( count == 1 ) break;
if( state_client && state_client.exists( ) && state_client.isValid( ) ) {
//ROS_INFO("Calling service");
if( state_client.call( s ) ) {
//ROS_INFO("Successful call");
ros::Time sim_time = ros::Time::now( );
//ROS_INFO( "sim_time: sec[%u] nsec[%u]", sim_time.sec, sim_time.nsec );
//ros::Duration current_time = sim_time - start_time;
//Real time = (Real) count / 1000.0;
Real time = (sim_time - start_time).toSec();
command.torque = control( time, s.response.position, s.response.velocity );
command.time = time;
ROS_INFO( "%10.9f, %10.9f, %10.9f, %10.9f", time, s.response.position, s.response.velocity, command.torque );
command_pub.publish( command );
//command.torque = control( state.response.time, state.response.position, state.response.velocity );
//command.time = state.response.time;
} else {
ROS_INFO("Failed to call");
}
} else {
ROS_INFO("Persistent client is invalid");
}
/*
if( !state_client.isValid( ) ) {
ROS_INFO( "state_client not valid" );
}
if( !state_client.exists( ) )
state_client.waitForExistence( );
if( state_client.call( s ) ) {
//if( state_client.call( state ) ) {
ROS_INFO( "called state" );
command.torque = control( s.response.time, s.response.position, s.response.velocity );
command.time = s.response.time;
//command.torque = control( state.response.time, state.response.position, state.response.velocity );
//command.time = state.response.time;
} else {
ROS_INFO( "calling state service failed" );
}
command_pub.publish( command );
*/
ros::spinOnce( );
count++;
}
ROS_INFO( "Controller shutting down" );
return 0;
}
int main(int argc, char **argv)
{
pending_bricks = std::vector<bpMsgs::robot_pick>();
/* ros messages */
/* parameters */
std::string brick_subscriber_topic;
std::string brick_publisher_topic;
std::string emergency_stop_subscriber_topic;
int loop_rate_param;
/* initialize ros usage */
ros::init(argc, argv, "robot_motion_controller");
ros::Publisher brick_publisher;
ros::Subscriber brick_subscriber;
ros::Subscriber emergency_stop_subscriber;
bpPLCController::plc_command plcCommand;
plcCommand.request.command_number = bpPLCController::plc_command::Request::GET_GRIPPER_SENSOR_STATUS;
/* private nodehandlers */
ros::NodeHandle nh;
ros::NodeHandle n("~");
/* read parameters from ros parameter server if available otherwise use default values */
n.param<std::string> ("brick_subscriber_topic", brick_subscriber_topic, "/bpRobotMotionController/brick_command_topic");
n.param<std::string> ("brick_publisher_topic", brick_publisher_topic, "/bpRobotMotionController/brick_response_topic");
n.param<std::string> ("emergency_stop_subscriber_topic", emergency_stop_subscriber_topic, "/emergency_stop");
n.param<int> ("loop_rate", loop_rate_param, 1);
brick_publisher = nh.advertise<bpMsgs::robot_pick> (brick_publisher_topic.c_str(), 10);
brick_subscriber = nh.subscribe<bpMsgs::robot_pick> (brick_subscriber_topic.c_str(), 20, brickHandler);
emergency_stop_subscriber = nh.subscribe<std_msgs::Bool> (emergency_stop_subscriber_topic.c_str(),20,emergencyStopHandler);
setKnownConfs();
robot_client = n.serviceClient<bpDrivers::command>("/bpDrivers/rx60_controller/rx60_command");
ros::ServiceClient plc_client = n.serviceClient<bpPLCController::plc_command>("/plc_controller/plc_command");
std::cout << "RobotMotionPlanner node started! - waiting for RX60 controller service" << std::endl;
robot_client.waitForExistence();
std::cout << "RX60 controller service found! - Going to idle position" << std::endl;
robot_state = idle;
// Go to Idle position
robot_client.call(cmdIdle,cmdRes);
robot_client.call(cmdOpenGripper, cmdRes);
bpMsgs::robot_pick brickToPick;
bpMsgs::robot_pick returnMessage;
ros::Rate loop_rate(loop_rate_param);
while (ros::ok())
{
ros::spinOnce();
switch (robot_state) {
case idle:
if (pending_bricks.size() > 0)
{
brickToPick = pending_bricks[0];
pending_bricks.erase(pending_bricks.begin());
if (!calcPositions(brickToPick))
{
// Cant reach brick
robot_state = failed;
}
else
{
robot_client.call(cmdWaitPosition,cmdRes);
ros::Duration(0.05).sleep();
robot_state = wait_for_wait_position;
}
}
else
{
robot_client.call(cmdIdle,cmdRes);
}
break;
case wait_for_wait_position:
cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED;
robot_client.call(cmdReq,cmdRes);
if (cmdRes.is_settled)
{
robot_state = go_down_to_pick_brick;
}
break;
case go_down_to_pick_brick:
if (brickToPick.header.stamp.toSec() < (ros::Time::now().toSec()) )
{
robot_state = failed;
}
else
{
ros::Duration( (brickToPick.header.stamp.toSec() - ros::Time::now().toSec()) - 0.20).sleep();
robot_client.call(cmdGripPosition,cmdRes);
robot_state = wait_for_brick;//wait_for_grip_position;
ros::Duration(0.05).sleep();
}
break;
case wait_for_grip_position:
cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED;
robot_client.call(cmdReq,cmdRes);
if (cmdRes.is_settled)
{
robot_state = wait_for_brick;
}
break;
case wait_for_brick:
if (brickToPick.header.stamp.toSec() < (ros::Time::now().toSec() - 0.0) )
{
robot_client.call(cmdCloseGripper,cmdRes);
ros::Duration(0.3).sleep();
plc_client.call(plcCommand);
ROS_INFO("Gripper1: %d", plcCommand.response.result);
if (!plcCommand.response.result)
{
robot_state = go_to_delivery_position;
}
else
{
robot_state = failed;
robot_client.call(cmdWaitPosition,cmdRes);
robot_client.call(cmdOpenGripper,cmdRes);
}
}
break;
case go_to_delivery_position:
robot_client.call(cmdWaitPosition,cmdRes);
robot_client.call(cmdOrder[brickToPick.order-1],cmdRes);
robot_client.call(cmdOrderDropoff[brickToPick.order-1],cmdRes);
robot_state = wait_for_delivery_position;
ros::Duration(0.1).sleep();
break;
case wait_for_delivery_position:
cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED;
robot_client.call(cmdReq,cmdRes);
if (cmdRes.is_settled)
{
plc_client.call(plcCommand);
ROS_INFO("Gripper2: %d", plcCommand.response.result);
if (!plcCommand.response.result)
{
robot_client.call(cmdOpenGripper,cmdRes);
ros::Duration(0.15).sleep();
robot_state = go_to_safe_position;
}
else
{
robot_client.call(cmdOpenGripper,cmdRes);
robot_client.call(cmdOrder[brickToPick.order-1],cmdRes);
ros::Duration(0.1).sleep();
robot_state = failed;
}
}
break;
case go_to_safe_position:
robot_client.call(cmdOrder[brickToPick.order-1],cmdRes);
ros::Duration(0.1).sleep();
robot_state = wait_for_safe_position;
break;
case wait_for_safe_position:
cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED;
robot_client.call(cmdReq,cmdRes);
if (cmdRes.is_settled)
{
robot_state = succeded;
}
break;
case succeded:
returnMessage = brickToPick;
returnMessage.succes = true;
brick_publisher.publish(returnMessage);
robot_state = idle;
break;
case failed:
ROS_INFO("ERROR cant reach brick");
returnMessage = brickToPick;
returnMessage.succes = false;
brick_publisher.publish(returnMessage);
robot_state = idle;
break;
case emergency_stop:
break;
default:
break;
}
// check if robot is settled
//cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED;
//client.call(cmdReq,cmdRes);
//if (cmdRes.is_settled);
loop_rate.sleep();
}
return 0;
}
int open_iob(void)
{
static bool isInitialized = false;
if ( isInitialized ) return TRUE;
isInitialized = true;
std::cerr << "[iob] Open IOB / start " << std::endl;
std::string node_name;
{
char *ret = getenv("HRPSYS_GAZEBO_IOB_NAME");
if (ret != NULL) {
node_name.assign(ret);
} else {
node_name = "hrpsys_gazebo_iob";
}
std::cerr << "[iob] set node name : " << node_name << std::endl;
}
std::map<std::string, std::string> arg;
ros::init(arg, "hrpsys_gazebo_iob", ros::init_options::NoSigintHandler);
rosnode = new ros::NodeHandle();
ros::WallDuration(0.5).sleep(); // wait for initializing ros
std::string controller_name;
{ // setting configuration name
char *ret = getenv("HRPSYS_GAZEBO_CONFIGURATION");
if (ret != NULL) {
controller_name.assign(ret);
} else {
controller_name = "hrpsys_gazebo_configuration";
}
ROS_INFO_STREAM( "[iob] set controller_name : " << controller_name);
}
std::string robotname;
{ // setting configuration name
char *ret = getenv("HRPSYS_GAZEBO_ROBOTNAME");
if (ret != NULL) {
robotname.assign(ret);
// controller_name -> robotname/controller_name
controller_name = robotname + "/" + controller_name;
} else {
std::string rname_str = std::string(controller_name) + "/robotname";
rosnode->getParam(rname_str, robotname);
}
if (robotname.empty()) {
ROS_ERROR("[iob] did not find robot_name");
robotname = "default";
}
ROS_INFO_STREAM( "[iob] set robot_name : " << robotname);
}
{ // setting synchronized
char *ret = getenv("HRPSYS_GAZEBO_IOB_SYNCHRONIZED");
if (ret != NULL) {
std::string ret_str(ret);
if (ret_str.size() > 0) {
iob_synchronized = true;
ROS_INFO("[iob] use synchronized command");
}
} else {
iob_synchronized = false;
}
}
{ // setting substeps
char *ret = getenv("HRPSYS_GAZEBO_IOB_SUBSTEPS");
if (ret != NULL) {
int num = atoi(ret);
if (num > 0) {
num_of_substeps = num;
ROS_INFO("[iob] use substeps %d", num);
}
}
}
joint_real2model_vec.resize(0);
if (rosnode->hasParam(controller_name + "/joint_id_list")) {
XmlRpc::XmlRpcValue param_val;
rosnode->getParam(controller_name + "/joint_id_list", param_val);
if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for(int i = 0; i < param_val.size(); i++) {
int num = param_val[i];
joint_real2model_vec.push_back(num);
}
} else {
ROS_WARN("[iob] %s/joint_id_list is not list of integer", controller_name.c_str());
}
} else {
ROS_DEBUG("[iob] %s/joint_id_list is nothing", controller_name.c_str());
}
if (rosnode->hasParam(controller_name + "/use_velocity_feedback")) {
bool ret;
rosnode->getParam(controller_name + "/use_velocity_feedback", ret);
use_velocity_feedback = ret;
if(ret) {
ROS_INFO("[iob] use_velocity_feedback");
}
}
XmlRpc::XmlRpcValue param_val;
std::vector<std::string> joint_lst;
rosnode->getParam(controller_name + "/joints", param_val);
if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for(unsigned int s = 0; s < param_val.size(); s++) {
std::string nstr = param_val[s];
ROS_DEBUG("add joint: %s", nstr.c_str());
joint_lst.push_back(nstr);
}
} else {
ROS_ERROR("[iob] %s/joints is not list of joint name", controller_name.c_str());
}
if (joint_real2model_vec.size() == 0) {
for(unsigned int i = 0; i < joint_lst.size(); i++) {
joint_real2model_vec.push_back(i);
}
} else if (joint_real2model_vec.size() != joint_lst.size()) {
ROS_ERROR("[iob] size differece on joint_id_list and joints (%ld, %ld)",
joint_real2model_vec.size(), joint_lst.size());
}
for(unsigned int i = 0; i < joint_real2model_vec.size(); i++) {
joint_model2real_map[joint_real2model_vec[i]] = i;
}
unsigned int n = NUM_OF_REAL_JOINT;
initial_jointcommand.position.resize(n);
initial_jointcommand.velocity.resize(n);
//initial_jointcommand.effort.resize(n);
initial_jointcommand.effort.resize(0);
if(!use_velocity_feedback) {
initial_jointcommand.kp_position.resize(n);
initial_jointcommand.ki_position.resize(n);
initial_jointcommand.kd_position.resize(n);
initial_jointcommand.kp_velocity.resize(n);
initial_jointcommand.i_effort_min.resize(n);
initial_jointcommand.i_effort_max.resize(n);
} else {
initial_jointcommand.kpv_position.resize(n);
initial_jointcommand.kpv_velocity.resize(n);
}
for (unsigned int i = 0; i < joint_lst.size(); ++i) {
std::string joint_ns(controller_name);
joint_ns += ("/gains/" + joint_lst[i] + "/");
if (!use_velocity_feedback) {
double p_val = 0, i_val = 0, d_val = 0, i_clamp_val = 0, vp_val = 0;
std::string p_str = std::string(joint_ns)+"p";
std::string i_str = std::string(joint_ns)+"i";
std::string d_str = std::string(joint_ns)+"d";
std::string i_clamp_str = std::string(joint_ns)+"i_clamp";
std::string vp_str = std::string(joint_ns)+"vp";
if (!rosnode->getParam(p_str, p_val)) {
ROS_WARN("[iob] couldn't find a P param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(i_str, i_val)) {
ROS_WARN("[iob] couldn't find a I param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(d_str, d_val)) {
ROS_WARN("[iob] couldn't find a D param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(i_clamp_str, i_clamp_val)) {
ROS_WARN("[iob] couldn't find a I_CLAMP param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(vp_str, vp_val)) {
ROS_WARN("[iob] couldn't find a VP param for %s", joint_ns.c_str());
}
// store these directly on altasState, more efficient for pub later
initial_jointcommand.kp_position[i] = p_val;
initial_jointcommand.ki_position[i] = i_val;
initial_jointcommand.kd_position[i] = d_val;
initial_jointcommand.i_effort_min[i] = -i_clamp_val;
initial_jointcommand.i_effort_max[i] = i_clamp_val;
initial_jointcommand.velocity[i] = 0;
//initial_jointcommand.effort[i] = 0;
initial_jointcommand.kp_velocity[i] = vp_val;
} else {
// velocity feedback
double p_v_val = 0, vp_v_val = 0;
std::string p_v_str = std::string(joint_ns) + "p_v";
std::string vp_v_str = std::string(joint_ns) + "vp_v";
if (!rosnode->getParam(p_v_str, p_v_val)) {
ROS_WARN("[iob] couldn't find a P_V param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(vp_v_str, vp_v_val)) {
ROS_WARN("[iob] couldn't find a VP_V param for %s", joint_ns.c_str());
}
initial_jointcommand.kpv_position[i] = p_v_val;
initial_jointcommand.kpv_velocity[i] = vp_v_val;
}
}
initial_jointcommand.desired_controller_period_ms =
static_cast<unsigned int>(g_period_ns * 1e-6);
if (iob_synchronized) {
serv_command =
ros::service::createClient<hrpsys_gazebo_msgs::SyncCommand> (robotname + "/iob_command", true); // persistent = true,
ROS_INFO_STREAM("[iob] waiting service " << robotname << "/iob_command");
serv_command.waitForExistence();
ROS_INFO_STREAM("[iob] found service " << robotname << "/iob_command");
} else {
pub_joint_command = rosnode->advertise <JointCommand> (robotname + "/joint_command", 1, true);
// ros topic subscribtions
ros::SubscribeOptions jointStatesSo =
ros::SubscribeOptions::create<RobotState>(robotname + "/robot_state", 1, setJointStates,
ros::VoidPtr(), rosnode->getCallbackQueue());
// Because TCP causes bursty communication with high jitter,
// declare a preference on UDP connections for receiving
// joint states, which we want to get at a high rate.
// Note that we'll still accept TCP connections for this topic
// (e.g., from rospy nodes, which don't support UDP);
// we just prefer UDP.
// temporary use TCP / Using UDP occured some problem when message size more than 1500.
//jointStatesSo.transport_hints =
//ros::TransportHints().maxDatagramSize(3000).unreliable().reliable().tcpNoDelay(true);
jointStatesSo.transport_hints =
ros::TransportHints().reliable().tcpNoDelay(true);
sub_robot_state = rosnode->subscribe(jointStatesSo);
}
for (int i=0; i < number_of_joints(); i++){
command[i] = 0.0;
power[i] = OFF;
servo[i] = OFF;
}
clock_gettime(CLOCK_MONOTONIC, &g_ts);
rg_ts = ros::Time::now();
jointcommand = initial_jointcommand;
std::cerr << "[iob] " << number_of_joints() << " / " << initial_jointcommand.position.size() << " / " << NUM_OF_REAL_JOINT << std::endl;
if (iob_synchronized) {
hrpsys_gazebo_msgs::SyncCommandRequest req;
req.joint_command = jointcommand;
hrpsys_gazebo_msgs::SyncCommandResponse res;
std::cerr << "[iob] first service call" << std::endl;
serv_command.call(req, res);
std::cerr << "[iob] first service returned" << std::endl;
js = res.robot_state;
init_sub_flag = true;
} else {
std::cerr << "[iob] block until subscribing first robot_state";
ros::Time start_tm = ros::Time::now();
ros::Rate rr(100);
ros::spinOnce();
while (!init_sub_flag) {
if ((ros::Time::now() - start_tm).toSec() > 5.0) {
std::cerr << "[iob] timeout for waiting robot_state";
break;
}
std::cerr << ".";
rr.sleep();
ros::spinOnce();
}
}
std::cerr << std::endl << "[iob] Open IOB / finish " << std::endl;
return TRUE;
}