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."); }
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()); } }
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); } }
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"); } }
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; } }