Exemplo n.º 1
0
bool checkNextStep() {
	printField();
	struct position nextPosition;
	nextPosition.i = currentPosition.i + shiftList[currentDirection][0];
	nextPosition.j = currentPosition.j + shiftList[currentDirection][1];

	enum state_type next;

	if (nextPosition.i < 0 || nextPosition.j < 0 || nextPosition.i >= field.cols ||
			nextPosition.j >= field.rows) {
		next = WALL;
	} else {
		next = field.m[currentPosition.i + shiftList[currentDirection][0]]
						[currentPosition.j + shiftList[currentDirection][1]];
	}

	if (next != NONE) {
		char res[25];
		currentSensor = next;

		if (currentState == GO) {
			currentState = STOP;
			sprintf(res, "Stopped: %s", sensor_type_str[currentSensor]);
		} else if (currentState == STOP) {
			sprintf(res, "The %s was detected", sensor_type_str[currentSensor]);
		}

		publishRobotState();
		publishMessage(res);
		return false;
	}

	currentSensor = next;
	return true;
}
bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses,
                                     const moveit::core::JointModelGroup* group)
{
  std::vector<std::string> tips;
  for (std::size_t i = 0; i < arm_datas_.size(); ++i)
    tips.push_back(arm_datas_[i].ee_link_->getName());

  // ROS_DEBUG_STREAM_NAMED(name_, "First pose should be for joint model group: " << arm_datas_[0].ee_link_->getName());

  const double timeout = 1.0 / 30.0;  // 30 fps

  // Optionally collision check
  moveit::core::GroupStateValidityCallbackFn constraint_fn;
  if (true)
  {
    bool collision_checking_verbose_ = false;
    bool only_check_self_collision_ = false;

    // TODO(davetcoleman): this is currently not working, the locking seems to cause segfaults
    // TODO(davetcoleman): change to std shared_ptr
    boost::scoped_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls;
    ls.reset(new planning_scene_monitor::LockedPlanningSceneRO(psm_));
    constraint_fn = boost::bind(&isIKStateValid, static_cast<const planning_scene::PlanningSceneConstPtr&>(*ls).get(),
                                collision_checking_verbose_, only_check_self_collision_, visual_tools_, _1, _2, _3);
  }

  // Solve
  std::size_t outer_attempts = 20;
  for (std::size_t i = 0; i < outer_attempts; ++i)
  {
    if (!imarker_state_->setFromIK(group, poses, tips, timeout, constraint_fn))
    {
      ROS_DEBUG_STREAM_NAMED(name_, "Failed to find dual arm pose, trying again");

      // Re-seed
      imarker_state_->setToRandomPositions(group);
    }
    else
    {
      ROS_DEBUG_STREAM_NAMED(name_, "Found IK solution");

      // Visualize robot
      publishRobotState();

      // Update the imarkers
      for (IMarkerEndEffectorPtr ee : end_effectors_)
        ee->setPoseFromRobotState();

      return true;
    }
  }

  ROS_ERROR_STREAM_NAMED(name_, "Failed to find dual arm pose");
  return false;
}
IMarkerRobotState::IMarkerRobotState(planning_scene_monitor::PlanningSceneMonitorPtr psm,
                                     const std::string& imarker_name, std::vector<ArmData> arm_datas,
                                     rviz_visual_tools::colors color, const std::string& package_path)
  : name_(imarker_name), nh_("~"), psm_(psm), arm_datas_(arm_datas), color_(color), package_path_(package_path)
{
  // Load Visual tools
  visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(
      psm_->getRobotModel()->getModelFrame(), nh_.getNamespace() + "/" + imarker_name, psm_);

  // visual_tools_->setPlanningSceneMonitor(psm_);
  visual_tools_->loadRobotStatePub(nh_.getNamespace() + "/imarker_" + imarker_name + "_state");

  // Load robot state
  imarker_state_ = std::make_shared<moveit::core::RobotState>(psm_->getRobotModel());
  imarker_state_->setToDefaultValues();

  // Create Marker Server
  const std::string imarker_topic = nh_.getNamespace() + "/" + imarker_name + "_imarker";
  imarker_server_ = std::make_shared<interactive_markers::InteractiveMarkerServer>(imarker_topic, "", false);

  // Get file name
  if (!getFilePath(file_path_, "imarker_" + name_ + ".csv", "config/imarkers"))
    exit(-1);

  // Load previous pose from file
  if (!loadFromFile(file_path_))
    ROS_INFO_STREAM_NAMED(name_, "Unable to find state from file, setting to default");

  // Show initial robot state loaded from file
  publishRobotState();

  // Create each end effector
  end_effectors_.resize(arm_datas_.size());
  for (std::size_t i = 0; i < arm_datas_.size(); ++i)
  {
    std::string eef_name;
    if (i == 0)
      eef_name = imarker_name + "_right";
    else
      eef_name = imarker_name + "_left";

    end_effectors_[i] = std::make_shared<IMarkerEndEffector>(this, eef_name, arm_datas_[i], color);

    // Create map from eef name to object
    name_to_eef_[eef_name] = end_effectors_[i];
  }

  // After both end effectors have been added, apply on server
  imarker_server_->applyChanges();

  ROS_DEBUG_STREAM_NAMED(name_, "IMarkerRobotState '" << name_ << "' Ready.");
}
Exemplo n.º 4
0
void handleCommand() {
	IOT_INFO("current state %d %s", currentState, state_type_str[currentState]);

	if (currentState == GO) {	// GO
		if (currentCommand != COMMAND_STOP) {
			publishMessage("Prohibited command. Only STOP available");
		} else {
			currentState = STOP;
			IOT_INFO("stopped");
			currentSensor = NONE;
			publishRobotState();
			publishMessage("Stopped");
		}
	} 
	else if (currentState == STOP) {				// STOP
		IOT_INFO("current command %d %s", currentCommand, protocol_str[currentCommand]);
		if (currentCommand == COMMAND_LEFT || currentCommand == COMMAND_RIGHT) {
				turn(currentCommand);
				publishRobotState();
				checkNextStep(); 
		} else if (currentCommand == COMMAND_GO) {
				if (checkNextStep()) {
				// 	makeStep();
					currentState = GO;
					// currentSensor = NONE;
					// publishRobotState();
				}
		} else if (currentCommand == COMMAND_STOP) {
			publishMessage("I am not moving");
		}
	}

	if (currentState == STOP) {
		publishMessage(getFieldAsString());
	}
}
Exemplo n.º 5
0
void makeStep() {
	if (currentState != STOP) {
		currentPosition.i += shiftList[currentDirection][0];
		currentPosition.j += shiftList[currentDirection][1];
		publishRobotState();

		char* fieldStr = getFieldAsString();
		char* res = (char*) malloc(MAX_LENGTH_OF_UPDATE_JSON_BUFFER*sizeof(char));
		strcpy(res, "");
		strcat(res, "I am going\n");
		strcat(res, fieldStr);
		publishMessage(res);
		free(res);
}
}
Exemplo n.º 6
0
void iot_subscribe_callback_handler(AWS_IoT_Client *pClient, char *topicName, uint16_t topicNameLen,
									IoT_Publish_Message_Params *params, void *pData) {
	IOT_UNUSED(pData);
	IOT_UNUSED(pClient);
	IOT_INFO("Message: %.*s\t%.*s", topicNameLen, topicName, (int) params->payloadLen, (char*)params->payload);

	char* msg = (char*)(malloc((int) params->payloadLen) + 1);
	strncpy(msg, (char*) params->payload, (int) params->payloadLen);
	msg[(int) params->payloadLen] = '\0';


	if (findCommand(msg)) {
		handleCommand();
	} else {
		publishRobotState();
		publishMessage("Undefined command");
	}
}
Exemplo n.º 7
0
int main(int argc, char **argv) {
	IoT_Error_t rc = FAILURE;
	parseInputArgsForConnectParams(argc, argv);

	if (!readDataFromFile()) {
		printf("Config file '%s' not found. Robot cannot work without configuration.\n", filename);
		return -1;
	} 
	else {
		if (connectToThingAndSubscribeToTopic(argc, argv)) {
			publishMessage(getFieldAsString());
			publishRobotState();

			while (true) {
				//Max time the yield function will wait for read messages
				rc = aws_iot_mqtt_yield(&mqttClient, 1000);

				if (NETWORK_ATTEMPTING_RECONNECT == rc) { // || NETWORK_ATTEMPTING_RECONNECT == rc1) {
				// If the client is attempting to reconnect we will skip the rest of the loop.
					continue;
				}

				sleep(3);

				if (currentState == GO) {
					if (checkNextStep()) {
						currentState = GO;
						currentSensor = NONE;
						makeStep();
					} 
				}
			}
		}

		return 0;
	}
}