TestKeyframeExtraction(ros::NodeHandle & nh) :
			action_client("/cloudbot1/turtlebot_move", true), map(
					new keyframe_map) {
		pointcloud_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZRGB> >(
				"/cloudbot1/pointcloud", 1);

		servo_pub = nh.advertise<std_msgs::Float32>(
				"/cloudbot1/mobile_base/commands/servo_angle", 3);

		keyframe_sub = nh.subscribe("/cloudbot1/keyframe", 10,
				&TestKeyframeExtraction::chatterCallback, this);

		update_map_service = nh.serviceClient<rm_localization::UpdateMap>(
				"/cloudbot1/update_map");

		clear_keyframes_service = nh.serviceClient<std_srvs::Empty>(
				"/cloudbot1/clear_keyframes");

		set_camera_info_service = nh.serviceClient<sensor_msgs::SetCameraInfo>(
				"/cloudbot1/rgb/set_camera_info");

		std_srvs::Empty emp;
		clear_keyframes_service.call(emp);

	}
Exemplo n.º 2
0
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 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;
}
Exemplo n.º 4
0
void startConveyerBelt(bool dir = false)
{
    rc_plc::StartConv obj;
    obj.request.direction = dir;
    if(!_serviceStart.call(obj))
        printConsole("Failed to call the 'serviceStartConveyer'");
}
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;
}
/**
 * @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;
      }
    }
  }

}
/**
 *  @brief Report to GCS arming / disarming state of the uav
 */
void state_callback(const mavros_msgs::State msg){
  uav_control::datalink_send ss;
  ss.request.source_id = sysid;
  ss.request.target_id = 0x00;
  ss.request.route = 2;

  ss.request.len = 4 + msg.mode.length() + 1;
  ss.request.payload[0] = 29;
  ss.request.payload[1] = msg.connected ? 'T' : 'F';
  ss.request.payload[2] = msg.armed ? 'T' : 'F';
  ss.request.payload[3] = msg.guided ? 'T' : 'F';
  sprintf((char*)&(ss.request.payload[4]), "%s", msg.mode.c_str());
  
  if(client.call(ss)){
    switch(ss.response.err){
      case 0: 
              break;
      case 1: ROS_ERROR("uav_stat_monitor sending failed: invalid channel");
              break;
      case 2: ROS_ERROR("uav_stat_monitor sending failed: channel not initialized");
              break;
      case 3: ROS_ERROR("uav_stat_monitor sending failed: sending failed");
              break;
      default: ROS_ERROR("uav_stat_monitor sending failed: unknown failure");
              break;
    }
  }
}
Exemplo n.º 8
0
bool CutterSteering::getFirstWayPoint()
{
  if (got_waypoint_)
    return true;

  waypoint_srv_.request.increment = false;
  if(waypoint_client_.call(waypoint_srv_))
  {
    if (waypoint_srv_.response.pointsLeft > 0)
    {
      got_waypoint_ = true; 
      ROS_INFO("Got first waypoint");

      initialX = map_pose_.position.x;
      initialY = map_pose_.position.y;
      ROS_INFO("Recorded initial position.");

      last_waypoint_ = cutter_msgs::WayPoint();
      target_waypoint_ = waypoint_srv_.response.currWayPoint;
      next_waypoint_ = waypoint_srv_.response.nextWayPoint;
      initial_align = true;

      return true;
    }
  }
  return false;
}
Exemplo n.º 9
0
        bool singleShotService(blort_ros::EstimatePose::Request &req,
                               blort_ros::EstimatePose::Response &resp)
        {
            if(lastImage.use_count() < 1 && lastCameraInfo.use_count() < 1)
            {
                ROS_ERROR("Service called but there was no data on the input topics!");
                return false;
            } else {

                ROS_INFO("Singleshot service has been called with a timeout of %f seconds.", time_to_run_singleshot);
                results.clear();

                if(parent_->tracker == 0)
                {
                    parent_->tracker = new blort_ros::GLTracker(*lastCameraInfo, parent_->root_, true);
                    parent_->recovery_client = parent_->nh_.serviceClient<blort_ros::RecoveryCall>("/blort_detector/poseService");
                } else {
                    parent_->tracker->reset();
                }
                parent_->tracker->setPublisMode(blort_ros::TRACKER_PUBLISH_GOOD);
                parent_->tracker->setVisualizeObjPose(true);
                blort_ros::SetCameraInfo camera_info;
                camera_info.request.CameraInfo = *lastCameraInfo;
                if(!detector_set_caminfo_service.call(camera_info))
                    ROS_ERROR("blort_tracker failed to call blort_detector/set_camera_info service");

                double start_secs = ros::Time::now().toSec();
                while(ros::Time::now().toSec()-start_secs < time_to_run_singleshot)
                {
                    ROS_INFO("Remaining time %f", time_to_run_singleshot+start_secs-ros::Time::now().toSec());
                    parent_->imageCb(lastImage);
                    if(parent_->tracker->getConfidence() == blort_ros::TRACKER_CONF_GOOD)
                    {
                        // instead of returning right away let's store the result
                        // to see if the tracker can get better
                        results.push_back(parent_->tracker->getDetections()[0]);
                    } else if(parent_->tracker->getConfidence() == blort_ros::TRACKER_CONF_LOST)
                    {
                        results.clear();
                    }
                }
                // we are out of the service call now, the results will be published
                inServiceCall = false;
                if(!results.empty())
                {
                    //convert results to a tf style transform and multiply them
                    //to get the camera-to-target transformation
                    resp.Pose = pal_blort::blortPosesToRosPose(parent_->tracker->getCameraReferencePose(),
                                                               results.back());
                    //NOTE: check the pose in vec3 location + mat3x3 rotation could be added here
                    // if we have any previous knowledge of the given scene
                    ROS_INFO_STREAM("PUBLISHED POSE: \n" << resp.Pose.position << "\n" <<
                                    pal_blort::quaternionTo3x3cvMat(resp.Pose.orientation) << "\n");
                    return true;
                } else {
                    //if the time was not enough to get a good detection, make the whole thing fail
                    return false;
                }
            }
        }
/*!
 * \brief set the position, speed and acceleration for a servo
 * \param index servo index
 * \param engage whether to energise the servo
 * \param position reference position for the servo
 * \param speed reference speed for the servo
 * \param acceleration reference acceleration for the servo
 */
void set_servo_reference(
						 int index,
						 bool engage,
						 float position,
						 float speed,
						 float acceleration)
{
	phidgets::servo_reference srv;
	srv.request.index = index;
	srv.request.engage = engage;
	srv.request.position = position;
	srv.request.speed = speed;
	srv.request.acceleration = acceleration;
	srv.response.ack = 0;
	if (client_servo_reference.call(srv)) {
		if ((int)srv.response.ack == 1) {
			ROS_INFO("Changed servo %d reference %.2f",
					 index, position);
		}
		else {
			ROS_INFO("Returned %d", (int)srv.response.ack);
		}
	}
	else {
		ROS_ERROR("Failed to call service servo_reference");
	}
}
/* Publishes an empty scene to reset/refresh it */
void resetOctomap(){
    cout << "Clearing Octomap !" << endl;
    moveit_msgs::PlanningScene planning_scene;

    moveit_msgs::GetPlanningScene scene_srv;
    scene_srv.request.components.components =   scene_srv.request.components.ALLOWED_COLLISION_MATRIX +
                                                scene_srv.request.components.LINK_PADDING_AND_SCALING +
                                                scene_srv.request.components.OBJECT_COLORS +
                                                scene_srv.request.components.OCTOMAP +
                                                scene_srv.request.components.ROBOT_STATE +
                                                scene_srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS +
                                                scene_srv.request.components.SCENE_SETTINGS +
                                                scene_srv.request.components.TRANSFORMS +
                                                scene_srv.request.components.WORLD_OBJECT_GEOMETRY +
                                                scene_srv.request.components.WORLD_OBJECT_NAMES;



    if(client_get_scene_.call(scene_srv)){
         moveit_msgs::PlanningScene planning_scene = scene_srv.response.scene;
         planning_scene.world.octomap.octomap.data.clear();
         ros::WallDuration sleep_time(0.5);
         //planning_scene.is_diff = true;
         planning_scene_publisher.publish(planning_scene);
         sleep_time.sleep();
    }
}
Exemplo n.º 12
0
void changeDirConveyerBelt(bool dir) // true = reverse
{
    rc_plc::ChangeDirection obj;
    obj.request.direction = dir;
    if(!_serviceChangeDir.call(obj))
        printConsole("Failed to call the 'serviceChangeDirConveyer'");
}
Exemplo n.º 13
0
/*
    Function to pick up an object named <object_name>
*/
void pick(std::string object_name)
{

    world_model_srv.request.base_frame = "gripperbot_base";
    world_model_srv.request.target_frame = object_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);
            opengripper();
            movegripperbot(world_model_srv.response.pose.position.x, world_model_srv.response.pose.position.y, world_model_srv.response.pose.position.z);
            closegripper();
            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");
    }

}
Exemplo n.º 14
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;
}
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;
}
Exemplo n.º 16
0
/*
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;
	}
}
Exemplo n.º 17
0
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);

}
		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_;
		}
Exemplo n.º 19
0
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();
    }
}
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;
}
brio_assembly_vision_new::Container * client_call(){
    brio_assembly_vision_new::Is_This_Easier srv;
    brio_assembly_vision_new::Container * cont = new brio_assembly_vision_new::Container();
    if (client.call(srv)) //call the service
    {
        int size=srv.response.get_message.date_container.size();
        if(size!=0){
            //use data
            for(int i=0;i<size;i++)
            {
                (*cont).date_container.push_back(srv.response.get_message.date_container.at(i));
            }

            ROS_INFO("Receved data. Number of clusters : %d",size);
            return cont;
        }
        //empty resonse
        ROS_ERROR("Failed to call OpenCV service , reason : RESPONSE IS EMPTY , retrying");
        return NULL;
    }
    else
    {
        ROS_ERROR("Failed to call OpenCV service , retrying");
        return NULL;
    }
}
Exemplo n.º 22
0
bool isRobotMoving()
{
    kuka_rsi::getIsMoving obj;
    if(!_serviceGetIsMoving.call(obj))
        printConsole("Failed to call the 'serviceIsRobotMoving'");

    return obj.response.isMoving;
}
Exemplo n.º 23
0
/**
 * Publishes the command to save the current map (from hector), on the server.
 */
void saveMap(){
	std_srvs::Empty srv;
	if(mapSaveCommandPublisher.call(srv)){
		cout << "SUCCESSFUL save response got from server. "<< endl;
	} else {
		cout << "UNSUCCESSFUL save response got from server. "<< endl;
	}
}
Exemplo n.º 24
0
void moveCoveyerBelt(double duration, bool dir = false)
{
    rc_plc::MoveConv obj;
    obj.request.duration = duration;
    obj.request.direction = dir;
    if(!_serviceMove.call(obj))
        printConsole("Failed to call the 'serviceMoveConveyer'");
}
/**
 * This tutorial demonstrates simple receipt of messages over the ROS system.
 */
void controlCallback(const sensor_msgs::Joy::ConstPtr& msg)
{
  //steer robot while holding dead man switch
  if(msg->buttons[4]) {
    t.linear.x = 0.9*t.linear.x + 0.1*l_scale_ * msg->axes[1];
    t.angular.z = 0.5*t.angular.z + 0.5*a_scale_ * msg->axes[0];
    interrupt_broadcasting = false;
    pub.publish(t);
  } else {
    t.linear.x = 0.0;
    t.angular.z = 0.0;
    if(interrupt_broadcasting == false){
       interrupt_broadcasting = true;
       pub.publish(t);
    }
  }
  #if WITH_SCITOS
	  //enable motors after bump and/or freerun
	  if(msg->buttons[7]) {
	    enable_srv.request.enable = true;
	    if (!enable_client.call(enable_srv))
	    {
	      ROS_ERROR("Failed to call service /enable_motors");
	    }
	    if (!reset_client.call(reset_srv))
	    {
	      ROS_ERROR("Failed to call service /reset_motorstop");
	    }
	  }
	  //disable motors to move robot manually
	  if(msg->buttons[6]) {
	    enable_srv.request.enable = false;
	    if (!enable_client.call(enable_srv))
	    {
	      ROS_ERROR("Failed to call service /enable_motors");
	    }
	  }
	  //emergency stop
	  if(msg->axes[2] == -1.0) {
	    if (!emergency_client.call(emergency_srv))
	    {
	      ROS_ERROR("Failed to call service /emergency_stop");
	    }
	  }
  #endif
}
Exemplo n.º 26
0
  bool unloadController(const std::string& name)
  {

    pr2_mechanism_msgs::UnloadController srv_msg;
    srv_msg.request.name = name;
    if (!unload_srv_.call(srv_msg)) return false;
    return srv_msg.response.ok;
  }
Exemplo n.º 27
0
void Align::move(double distance){
        

        //double seconds = distance / velocity;
        //double angular_velocity = velocity / WHEEL_RAD;

	double seconds = distance / 0.09;
	seconds = seconds - (12*(seconds / 100));

	//stop
    diffDrive.call(msg_forward);

        ros::Duration(seconds).sleep();

        // stop
        diffDrive.call(msg_stop);
}
Exemplo n.º 28
0
 void set_control_mode(unsigned int mode, double elevation=M_PI/2) {
     hdpc_drive::SetControlMode req;
     req.request.request_mode = mode;
     req.request.desired_elevation = elevation;
     if (!setModeClt.call(req)) {
         ROS_ERROR("Set control mode %d failed",mode);
     }
 }
/*
 * Method that, by receiving a string, makes the robot talk that string
 */
void speak_this(string to_speak)
{
	srv_talker.request.sentence = "Hello"; ///to_speak

	if (client_talker.call(srv_talker)) //srv_talker
		ROS_INFO("Talked: %s", to_speak.c_str());
	else
		ROS_ERROR("Failed to call the service of qbo_talk");
}
//=============================================================
//名前:InitScan
//説明:首振りの初期位置合わせ
//=============================================================
void ThermalScanning::InitScan(void)
{
	srv.request.speed = MOTOR_SPEED;	
	dyna_client.call(srv);
		
	// ダイナミクセルの角度を初期位置に移動する
	Dyna_angle = 0.0;
	tilt_controller_command_msg.data = Dyna_angle;
}