int main(int argc, char **argv) { ros::init(argc, argv, "rps_robot_commander"); ros::NodeHandle n; //~ ros::Subscriber rps_map_subscriber = n.subscribe("rps_map_data", 1, set_RPS_MAP); ros::ServiceServer server_robot_command = n.advertiseService("rps_robot_command", start_robot_commander); commander_to_get_robots_info = n.serviceClient<tms_msg_db::tmsdb_get_robots_info>("tmsdb_get_robots_info"); #ifdef SEND_COMMAND client_smartpal5_control = n.serviceClient<tms_msg_rc::smartpal_control>("sp5_control"); #endif client_smartpal5_speak = n.serviceClient<tms_msg_rc::smartpal_speak>("sp5_speak"); //////////////////////////// #ifdef USE_TMS_DB srv_r.request.robots_id = 2; if(commander_to_get_robots_info.call(srv_r)) ROS_INFO("Success robots_x = %lf, y = %lf, theta = %lf", srv_r.response.robots_x,srv_r.response.robots_y,srv_r.response.robots_theta); else{ ROS_ERROR("Failed to call service get_robots_info\n"); return 0; } srv_smartpal_control.request.unit = UNIT_VEHICLE; srv_smartpal_control.request.cmd = CMD_setPose; srv_smartpal_control.request.arg.resize(3); srv_smartpal_control.request.arg[0] = srv_r.response.robots_x; srv_smartpal_control.request.arg[1] = srv_r.response.robots_y; srv_smartpal_control.request.arg[2] = srv_r.response.robots_theta; #else srv_smartpal_control.request.unit = UNIT_VEHICLE; srv_smartpal_control.request.cmd = CMD_setPose; srv_smartpal_control.request.arg.resize(3); srv_smartpal_control.request.arg[0] = 1000; srv_smartpal_control.request.arg[1] = 1000; srv_smartpal_control.request.arg[2] = 0.0; #endif #ifdef SEND_COMMAND if(client_smartpal5_control.call(srv_smartpal_control)){ ROS_INFO("result: %d", srv_smartpal_control.response.result); } else{ ROS_ERROR("Failed to call service sp5_control"); return 0; } #endif //////////////////////////// ros::spin(); return 0; }
void emergencyStopHandler(std_msgs::Bool msg) { if (msg.data) { pending_bricks.clear(); robot_client.call(cmdResetMotion,cmdRes); robot_state = emergency_stop; } else { robot_client.call(cmdOpenGripper,cmdRes); robot_state = idle; } }
void joint_state_cb(const sensor_msgs::JointStateConstPtr& js){ if (js->position.size() > 4){ //Message from the base or the arm js_cur = *js; last_status = sbs; if(!checkIfSafe()) sbs.status = sbs.PAUSED; else sbs.status = sbs.RUNNING; sb_req.status = sbs; sb_req.requester = "mico_safety_node"; if(sbs.status != last_status.status){ if(stopbase_client.call(sb_req,sb_resp)){ ROS_DEBUG("Made stop_base call"); } else{ ROS_ERROR("Stop base service call failed!"); ros::shutdown(); } } } };
/* * Check the busy flag * * Poll for the busy flag returns true if busy, false if not */ bool isADCBusy(ADC adc) { //Define and fill in a service request hardware_interface::ReadI2CRegister read; read.request.addr = adc; read.request.reg = ADC_BUSY; read.request.size = 1; //Send request if(read_register_svr.call(read)) { //Sussecful read if(read.response.data[0]) { //Still busy return true; } else { //Not busy! return false; } } else { //Unsussesful read!!! //Do error handeling } return false; }
void moveToJointState(const float* js){ /*moveit::planning_interface::MoveGroup group("arm"); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; group.setPlanningTime(5.0); //10 second maximum for collision computation*/ moveit_utils::MicoMoveitJointPose::Request req; moveit_utils::MicoMoveitJointPose::Response res; for(int i = 0; i < 6; i++){ switch(i) { case 0 : req.target.joint1 = js[i]; break; case 1 : req.target.joint2 = js[i]; break; case 2 : req.target.joint3 = js[i]; break; case 3 : req.target.joint4 = js[i]; break; case 4 : req.target.joint5 = js[i]; break; case 5 : req.target.joint6 = js[i]; break; } //ROS_INFO("Requested angle: %f", q_vals.at(i)); } if(client_joint_command.call(req, res)){ ROS_INFO("Call successful. Response:"); ROS_INFO_STREAM(res); } else { ROS_ERROR("Call failed. Terminating."); //ros::shutdown(); } }
void jointsCall() { //ここをどうするか? for(int i = 0; i < okao_id.size(); ++i) { humans_msgs::HumanImgSrv hs; //hs.request.rule = rule; hs.request.src.max_okao_id = okao_id[i]; //okao_client::OkaoStack okao_img; //okao_img.request.person.okao_id = okao_id; if( srv.call( hs ) ) { //cout << "okao_id:" << okao_id[i] << ", x,y =" // << hs.response.dst.p.x << ","<< hs.response.dst.p.y << endl ; markerPublisher( hs.response.dst, hs.response.img ); } else { cout << "not found!"<<endl; } } }
/* processData is the main decision function that * will publish commands to the motor controller * node and possibly ask for arduino data. */ void processData() { //geometry_msgs::Twist command; beagle_pkg::status status; if(ard_client.call(ard_data)) { ROS_INFO("lidar data was %f, arduino reading was %f\n", scan_data.range_min, ard_data.response.distance); if(ard_data.response.distance > 5 && scan_data.range_min > 5) { status.command = 0; } else if(ard_data.response.distance <= 0 && scan_data.range_min <= 0) { status.command = 0; } else if(ard_data.response.distance > scan_data.range_min) { status.command = 1; } else { status.command = 2; } /* TODO: look at global data (scan_data, gps_data, ard_data) * and make decision of what to send to the motor node. */ vel_pub.publish(status); } else { ROS_ERROR("Failed to call ard service"); } }
//============================================================= //名前:TurnScan //説明:首振りの方向折り返し //============================================================= void ThermalScanning::TurnScan(void) { //std_msgs::Float64 tilt_controller_command_msg; if(cw == -1) // 左回りの場合 { start_angle = MIN_YAW_ANGLE_CAM; end_angle = MAX_YAW_ANGLE_CAM; pm = 1; }else if(cw == 1){ // 右回りの場合 start_angle = MAX_YAW_ANGLE_CAM; end_angle = MIN_YAW_ANGLE_CAM; pm = -1; } // ダイナミクセルのスピードの設定 if(Scan_state == 0 || Scan_state == 5 || Scan_state == 6) // 通常探索 { srv.request.speed = MOTOR_SPEED; } else if(Scan_state == 1) // 強化探索 { srv.request.speed = MOTOR_SPEED2; } dyna_client.call(srv); // ダイナミクセルの角度を初期位置に移動する // TODO ダイナミクセルの角度をDyna_angleに変更する処理を入れる Dyna_angle = start_angle; tilt_controller_command_msg.data = Dyna_angle; }
int main(int argc, char **argv){ ros::init(argc, argv, "run_pose_estimation_for_scene_screenshot"); ros::NodeHandle nodeHandle = ros::NodeHandle("~"); global = nodeHandle.serviceClient<MsgT>("/inSceneDetector/detect"); if (!once){ //get screen shot pose ROS_INFO("Subscribing to detection service for screen shot pose..."); ros::service::waitForService("/inSceneDetector/detect"); MsgT msgglobal01; msgglobal01.request.scenario = "detect_screen_shot"; if(global.call(msgglobal01)) { //printResults(msgglobal01); pcl::console::print_value("Screenshot detected!\n"); } else pcl::console::print_error("Screenshot not detected!\n"); once = true; } ros::spinOnce(); return 0; }
std::vector<Brick> getBricks() { std::vector<Brick> retVec; rc_vision::getBricks obj; if(!_serviceGetBricks.call(obj)) { printConsole("Failed to call the 'serviceGetBricks'"); } else { for(unsigned int i=0; i<obj.response.size.size(); i++) { Brick brick; brick.color = obj.response.color[i]; brick.posX = obj.response.posX[i]; brick.posY = obj.response.posY[i]; brick.size = obj.response.size[i]; brick.theta = obj.response.theta[i]; retVec.push_back(brick); } } return retVec; }
void anyBrickCallback(std_msgs::Bool msg) { // Fetch robot position kuka_rsi::getConfiguration getQObj; // Call service if(!_serviceGetConf.call(getQObj)) printConsole("Failed to call the 'serviceKukaGetConfiguration'"); // Get information bool same = true; for(int i=0; i<6; i++) { if(fabs(getQObj.response.q[i]*DEGREETORAD - _qIdle[i]) > 0.1) { same = false; break; } } _anyBricksMutex.lock(); _anyBricks = msg.data; _anyBricksMutex.unlock(); _qMutex.lock(); _positionQIdle = same; _qMutex.unlock(); }
void changeDirConveyerBelt(bool dir) // true = reverse { rc_plc::ChangeDirection obj; obj.request.direction = dir; if(!_serviceChangeDir.call(obj)) printConsole("Failed to call the 'serviceChangeDirConveyer'"); }
void startConveyerBelt(bool dir = false) { rc_plc::StartConv obj; obj.request.direction = dir; if(!_serviceStart.call(obj)) printConsole("Failed to call the 'serviceStartConveyer'"); }
/** * @brief Callback functions for datalink routing, channel 1 * */ void sub1_callback(const uav_control::DFrame msg) { // message from GCS, to be relayed to quadcopter if(msg.target_id != SYSID){ uav_control::datalink_send ss; ss.request.source_id = msg.source_id; ss.request.target_id = msg.target_id; ss.request.route = msg.route; ss.request.len = msg.len; int i; for(i=0; i<msg.len; i++) ss.request.payload[i] = msg.payload[i]; if(client_sub1.call(ss)){ switch(ss.response.err){ case 0: //ROS_INFO("successful"); break; case 1: ROS_ERROR("relay error: invalid channel"); break; case 2: ROS_ERROR("relay error: channel not initialized"); break; case 3: ROS_ERROR("relay error: sending failed"); break; default: ROS_ERROR("relay error: unknown failure"); break; } } } }
bool plotSummary(ros::ServiceClient& sum_client) { PlanningSummary planning_sum; if (!sum_client.call(planning_sum)) { return false; } unsigned int cols = planning_sum.response.score_labels.size(); for (unsigned int i = 0; i < planning_sum.response.score_labels.size(); i++) { cout << planning_sum.response.score_labels[i] << "\t"; } cout << endl; for (unsigned int i = 0; i < planning_sum.response.score_values.size(); i++) { if (i % cols == 0) { cout << i / cols << ": "; } cout << planning_sum.response.score_values[i]; cout << "\t"; if (i % cols == cols - 1) { cout << endl; } } return true; }
void TrajectoryMapWriter::draw(MapWriterInterface *interface) { if(!initialized_) return; hector_nav_msgs::GetRobotTrajectory srv_path; if (!service_client_.call(srv_path)) { ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str()); return; } std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses); size_t size = traj_vector.size(); std::vector<Eigen::Vector2f> pointVec; pointVec.resize(size); for (size_t i = 0; i < size; ++i){ const geometry_msgs::PoseStamped& pose (traj_vector[i]); pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y); } if (size > 0){ //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z); Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f); interface->drawPath(startVec, pointVec); } }
void testIKServiceCall() { // define goal pose for right hand end effector Eigen::Affine3d goal_pose = Eigen::Affine3d::Identity(); goal_pose *= Eigen::AngleAxisd(3.141593, Eigen::Vector3d::UnitY()); goal_pose.translation()[0] = 0.6; goal_pose.translation()[1] = -0.5; goal_pose.translation()[2] = 0.1; visual_tools_->publishAxisLabeled(goal_pose, "goal_pose"); // call ik service geometry_msgs::PoseStamped tf2_msg; tf2_msg.header.stamp = ros::Time::now(); tf::poseEigenToMsg(goal_pose, tf2_msg.pose); ik_srv_.request.pose_stamp.push_back(tf2_msg); // TODO: not the right way to call this? compile error on this section... if (ik_client_right_.call(ik_srv_)) { ROS_DEBUG_STREAM_NAMED("testIKServiceCall","ik service called successfully."); } else { ROS_WARN_STREAM_NAMED("testIKServiceCall","Failed to call ik service..."); } }
bool sendAndMonitorTask() { ArmRequest req; ArmResponse res; uint32_t srv_call_counter = 0; bool succeeded = false; std::vector<TaskDetails> recovery_tasks; req.tasks_codes = tasks_codes_; while((srv_call_counter++ < attempts_) && !succeeded) { ROS_INFO_STREAM(ros::this_node::getName()<<": Task '"<<name_<<"' attempt "<<srv_call_counter); succeeded = arm_client_.call(req,res) && (bool)res.completed; ROS_INFO_STREAM(ros::this_node::getName()<<": Task '"<<name_<<"' state " <<(succeeded ?" succeeded":" failed")); } error_code_ = res.error_code; // on failure execute recovery task serially if(!succeeded && getRecoveryTasks(recovery_tasks)) { std::vector<TaskDetails>::iterator i; for(i = recovery_tasks.begin(); i != recovery_tasks.end(); i++) { if(!i->sendAndMonitorTask() ) { // checking termination error, and exiting recovery_task_termination_error_ = i->terminationErrorReceived(); break; } } } completed_ = succeeded; return completed_; }
unsigned int nrControllers() { pr2_mechanism_msgs::ListControllers srv_msg; if (!list_srv_.call(srv_msg)) return 0;; return srv_msg.response.controllers.size(); }
void heartMonitor(ros::NodeHandle n, ros::ServiceClient client, estop_control::estopSignal srv) { time(&now); // get current time; same as: now = time(NULL) if(heartbeat == true) { heartbeat = false; ROS_INFO("beating"); previousTime = now; } if(difftime(now, previousTime) > 1) // 1 second delay { ROS_INFO("heart stopped"); //estop // ros::ServiceClient client = n.serviceClient<estop_control::estopSignal>("estop_control"); // estop_control::estopSignal srv; srv.request.message = 1; if (client.call(srv)) { ROS_INFO("Recieved handshake: %d", (bool)srv.response.handshake); } else { ROS_ERROR("Failed to call service estop_control"); } //client.shutdown(); } }
void callback(const nav_msgs::Odometry::ConstPtr &odom) { if(isnan(last_point.x) && isnan(last_point.y) && isnan(last_point.z)) { last_point = odom->pose.pose.position; return; } double distance = sqrt(pow(odom->pose.pose.position.x-last_point.x,2)+pow(odom->pose.pose.position.y-last_point.y,2)); total_distance += distance; std_msgs::Float32 pub_dist; pub_dist.data = (float) total_distance; mileage_pub.publish(pub_dist); last_point = odom->pose.pose.position; if(save % save_interval == 0) { ros::param::set("/saved_mileage",total_distance); srv.request.param = "saved_mileage"; if (client.call(srv)) ROS_DEBUG("Save mileage: success"); else ROS_WARN("Save mileage: failed"); save = 0; } save++; }
bool callService::callTheService() { if(client_.call(srv_)){ double x,y,z,qx,qy,qz,qw; x = srv_.response.final_pose.position.x; y = srv_.response.final_pose.position.y; z = srv_.response.final_pose.position.z; qx = srv_.response.final_pose.orientation.x; qy = srv_.response.final_pose.orientation.y; qz = srv_.response.final_pose.orientation.z; qw = srv_.response.final_pose.orientation.w; ROS_INFO("Pose: tx= %5.4lf %5.4lf %5.4lf quat= %5.3lf %5.3lf %5.3lf %5.3lf, cost= %5.3lf", srv_.response.final_pose.position.x, srv_.response.final_pose.position.y, srv_.response.final_pose.position.z, srv_.response.final_pose.orientation.x, srv_.response.final_pose.orientation.y, srv_.response.final_pose.orientation.z, srv_.response.final_pose.orientation.w, srv_.response.final_cost_per_observation); tf::Transform camera_to_target; tf::Quaternion quat(qx,qy,qz,qw); camera_to_target.setOrigin(tf::Vector3(x,y,z)); camera_to_target.setRotation(quat); tf::StampedTransform stf(camera_to_target, ros::Time::now(), optical_frame_.c_str(), "target_frame"); tf_broadcaster_.sendTransform(stf); return(true); } ROS_ERROR("Target Location Failure"); return(false); }
virtual void Update( ) { /* common::Time sim_time = world->GetSimTime(); Real time = (sim_time - start_time).Double(); Real position = this->joints[0]->GetAngle( 0 ).Radian( ); Real velocity = this->joints[0]->GetVelocity( 0 ); Real torque = control( time, position, velocity ); this->joints[0]->SetForce( 0, torque ); last_time = sim_time; */ gazebo::common::Time sim_time = world->GetSimTime(); pendulum::rpc_command state; state.request.time = (sim_time - start_time).Double(); state.request.position = this->joints[0]->GetAngle( 0 ).Radian( ); state.request.velocity = this->joints[0]->GetVelocity( 0 ); if( client.call(state) ) { Real torque = state.response.torque; this->joints[0]->SetForce( 0, torque ); } last_time = sim_time; }
rvMCUReport getReport() { rvMCUReport rv; if (reportClient.call(reportSrv)) { rv.length = reportSrv.response.length; rv.lDirection = reportSrv.response.lDirection; rv.lNum = reportSrv.response.lNum; rv.rDirection = reportSrv.response.rDirection; rv.rNum = reportSrv.response.rNum; rv.rearDirection = reportSrv.response.rearDirection; rv.rearNum = reportSrv.response.rearNum; rv.headPosition = reportSrv.response.headPosition; rv.isLedOn = reportSrv.response.isLedOn; rv.isIrOn = reportSrv.response.isIrOn; rv.isDetectedBarrier = reportSrv.response.isDetectedBarrier; ROS_INFO("MCU Report:\nlength=%d", rv.length); ROS_INFO("Left direction:num=%d:%d", rv.lDirection, rv.lNum); ROS_INFO("Right direction:num=%d:%d", rv.rDirection, rv.rNum); ROS_INFO("Rear direction:num=%d:%d", rv.rearDirection, rv.rearNum); ROS_INFO("headPosition=%d", rv.headPosition); ROS_INFO("isLedOn=%d,isIrOn=%d,isDetectedBarrier=%d", rv.isLedOn, rv.isIrOn, rv.isDetectedBarrier); } else { ROS_ERROR("Failed to call service rovioReport"); } return rv; }
void send_reset() { hdpc_com::ChangeStateMachine change_state; change_state.request.event = EVENT_RESET; if (!state_machine_client.call(change_state)) { ROS_WARN("Change state machine: reset request failed"); } }
/* createSimulatePlaneCallback This is the callback used to handle any users wishing to create a new plane. This could be used for setting up a course as well. */ bool createSimulatedPlaneCallback(AU_UAV_ROS::CreateSimulatedPlane::Request &req, AU_UAV_ROS::CreateSimulatedPlane::Response &res) { //create a service to get a plane ID to use AU_UAV_ROS::RequestPlaneID srv; srv.request.requestedID = req.requestedID; //check to make sure the client call worked (regardless of return values from service) if(requestPlaneIDClient.call(srv)) { res.planeID = srv.response.planeID; if(srv.response.planeID == -1) { ROS_ERROR("Couldn't create plane with ID %d", req.requestedID); return false; } //create and add our plane to the list of simulated planes //AU_UAV_ROS::SimulatedPlane newPlane(srv.response.planeID, req); simPlaneMap[srv.response.planeID] = AU_UAV_ROS::SimulatedPlane(srv.response.planeID, req); //plane created successfully return true; } else { //if this happens, chances are the coordinator isn't running ROS_ERROR("Did not receive an ID from coordinator"); return false; } }
void ubicacionCallback(const vrep_common::ObjectGroupData msgUbicacionPatas) { for(int k=1; k<Npatas+1; k++){ ubicacionRobot.coordenadaPata_x[k-1]=msgUbicacionPatas.floatData.data[0+k*Npatas/2]; ubicacionRobot.coordenadaPata_y[k-1]=msgUbicacionPatas.floatData.data[1+k*Npatas/2]; ubicacionRobot.coordenadaPata_z[k-1]=msgUbicacionPatas.floatData.data[2+k*Npatas/2]; //-- Transformacion de trayectoria a Sistema de Pata srv_Trans_MundoPata.request.modo=Mundo_Pata; srv_Trans_MundoPata.request.Npata=k-1; srv_Trans_MundoPata.request.x_S0=ubicacionRobot.coordenadaPata_x[k-1]; srv_Trans_MundoPata.request.y_S0=ubicacionRobot.coordenadaPata_y[k-1]; srv_Trans_MundoPata.request.z_S0=ubicacionRobot.coordenadaPata_z[k-1]; srv_Trans_MundoPata.request.x_UbicacionRob=ubicacionRobot.coordenadaCuerpo_x; srv_Trans_MundoPata.request.y_UbicacionRob=ubicacionRobot.coordenadaCuerpo_y; srv_Trans_MundoPata.request.theta_Rob=ubicacionRobot.orientacionCuerpo_yaw; if (client_Trans_MundoPata.call(srv_Trans_MundoPata)) { ubicacionRobot.coordenadaPataSistemaPata_x[k-1] = srv_Trans_MundoPata.response.x_Pata; ubicacionRobot.coordenadaPataSistemaPata_y[k-1] = srv_Trans_MundoPata.response.y_Pata; ubicacionRobot.coordenadaPataSistemaPata_z[k-1] = srv_Trans_MundoPata.response.z_Pata; // ROS_INFO("Servicio motores: q1=%.3f; q2=%.3f; q3=%.3f\n", _q1, _q2, _q3); } else { ROS_ERROR("Nodo6:[%d] servicio de Trans_MundoPata no funciona\n",k-1); // return; } // ROS_INFO("Nodo6: pata[%d], x=%.3f, y=%.3f",k-1,ubicacionRobot.coordenadaPata_x[k-1],ubicacionRobot.coordenadaPata_y[k-1]); } infoPatas = true; }
/* Function to place the currently grasped object on the object named <container_name> */ void place(std::string container_name) { world_model_srv.request.base_frame = "gripperbot_base"; world_model_srv.request.target_frame = container_name; if(world_model_client.call(world_model_srv)) { if(world_model_srv.response.success) { if(world_model_srv.response.pose.position.x < 0) return; if(world_model_srv.response.pose.position.z < 0) world_model_srv.response.pose.position.z = -world_model_srv.response.pose.position.z; movegripperbot(world_model_srv.response.pose.position.x, world_model_srv.response.pose.position.y, world_model_srv.response.pose.position.z+0.1); movegripperbot(world_model_srv.response.pose.position.x, world_model_srv.response.pose.position.y, world_model_srv.response.pose.position.z); opengripper(); movegripperbot(world_model_srv.response.pose.position.x, world_model_srv.response.pose.position.y, world_model_srv.response.pose.position.z+0.1); } else { ROS_ERROR("Transform between object and gripperbot_base doesnt exist"); } } else { ROS_ERROR("Failed to call service /world_model/get_transform"); } }
void moveMouse(ros::ServiceClient& client, const cv::Point2f& goal) { agario_mouse::Mouse srv; srv.request.x = (int)goal.x + MOUSE_CALIBRATE_X; srv.request.y = (int)goal.y + MOUSE_CALIBRATE_Y; client.call(srv); }
bool rotate_cb(mantis_data_collection::dataCollect::Request &req, mantis_data_collection::dataCollect::Response &res) { std_msgs::UInt16 command; command.data=0; mantis_data_collection::process_cloud srv; srv.request.objectName = req.objectName; //following line added by CG along with following // comments to make useful for one angle command.data = req.delta; //while(command.data<360) //{ //ros::Rate loop_rate(.1); srv.request.in_cloud = cloud_to_process; srv.request.angle = command.data; pan_client.call(srv); srv.response.result = 1; pan_pub.publish(command); //std::cout << "Angle " << srv.request.angle << "\n"; //ros::spinOnce(); //loop_rate.sleep(); //command.data += req.delta; //} res.result = 1; std::cout << "Status: " << srv.response.result << "\n"; return 1; }