Exemplo n.º 1
0
	void initialize()
		{


			// write "right" to ltl.planner
			std_msgs::String msg;
			std::stringstream ss;
			ss << "right" << std::endl;
			std::cout << "**AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAGoToWaypoint -%- Executing Main Task, elapsed_time: " << std::endl;

			msg.data = ss.str();
			// ROS_INFO("%s", msg.data.c_str());
			chatter_pub.publish(msg);
			std::cout << "**msg:"
			          << msg << std::endl;
			counter++;

			sleep(1.0);
			//Set the stiffness so the robot can move
			AL::ALValue stiffness_name("Body");
			AL::ALValue stiffness(1.0f);
			AL::ALValue stiffness_time(1.0f);
			motion_proxy_ptr->stiffnessInterpolation(stiffness_name,
			                                         stiffness,
			                                         stiffness_time);
			if( message_.type == "goto")
				{
				 motion_proxy_ptr->moveInit();
				}
			set_feedback(RUNNING);
		}
	void initialize()
	{
		init_ = true;

		// Enable stiffness
		AL::ALValue stiffness_name("Body");
		AL::ALValue stiffness(1.0f);
		AL::ALValue stiffness_time(1.0f);
		motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time);

		// Init moving
		motion_proxy_ptr->moveInit();

		// Robot not detected
		robotDetected = false;
	}
Exemplo n.º 3
0
	void initialize()
	{
		init_ = true;

		// Enable stiffness
		AL::ALValue stiffness_name("Body");
		AL::ALValue stiffness(1.0f);
		AL::ALValue stiffness_time(1.0f);
		motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time);

		// Stand
		//robotPosture->goToPosture("Stand",0.5f);

		// Init moving
		motion_proxy_ptr->moveInit();

		// Initial Odometry
		x_0 = motion_proxy_ptr->getRobotPosition(true).at(0);
		y_0 = motion_proxy_ptr->getRobotPosition(true).at(1);
		z_0 = motion_proxy_ptr->getRobotPosition(true).at(2);
	}