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; }