MotorController::MotorController(ros::NodeHandle* nodehandle) : nh_(*nodehandle) { // initialize private node handle ros::NodeHandle pNh("~"); // initialize subscriber to /motors topic cmd_sub_ = nh_.subscribe("/motors", 1, &MotorController::cmdCallback, this); // initialize publishers to publish mbed stats enc_pub_ = nh_.advertise<igvc_msgs::velocity_pair>("/encoders", 1000); enabled_pub_ = nh_.advertise<std_msgs::Bool>("/robot_enabled", 1); battery_pub_ = nh_.advertise<std_msgs::Float64>("/battery", 1); // get server ip address and port number from the launch file igvc::getParam(pNh, std::string("ip_addr"), ip_addr_); igvc::getParam(pNh, std::string("port"), tcpport_); ROS_INFO_STREAM("Connecting to server:" << "\n\tIP: " << ip_addr_ << "\n\tPort: " << std::to_string(tcpport_)); sock_ = igvc::make_unique<EthernetSocket>(ip_addr_, tcpport_); ROS_INFO_STREAM("Using Boost " << (*sock_).getBoostVersion()); ROS_INFO_STREAM("Successfully Connected to TCP Host:" << "\n\tIP: " << (*sock_).getIP() << "\n\tPort: " << (*sock_).getPort()); igvc::getParam(pNh, std::string("battery_alpha"), battery_alpha_); igvc::getParam(pNh, std::string("min_battery_voltage"), min_battery_voltage_); battery_avg_ = min_battery_voltage_; // PID variables igvc::getParam(pNh, std::string("p_l"), p_l_); igvc::getParam(pNh, std::string("p_r"), p_r_); igvc::getParam(pNh, std::string("d_l"), d_l_); igvc::getParam(pNh, std::string("d_r"), d_r_); igvc::getParam(pNh, std::string("i_r"), i_r_); igvc::getParam(pNh, std::string("i_l"), i_l_); igvc::getParam(pNh, "kv_l", kv_l_); igvc::getParam(pNh, "kv_r", kv_r_); igvc::param(pNh, "log_period", log_period_, 5.0); // communication frequency igvc::getParam(pNh, std::string("frequency"), frequency_); ros::Rate rate(frequency_); setPID(); // Set PID Values on mbed // send motor commands while (ros::ok()) { sendRequest(); recieveResponse(); ros::spinOnce(); rate.sleep(); } }
int main(int argc, char** argv) { ros::init(argc, argv, "scan_to_pointcloud"); ros::NodeHandle pNh("~"); pNh.getParam("min_dist", min_dist); pNh.getParam("neighbor_dist", neighbor_dist); ros::NodeHandle nh; _pointcloud_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/pc2", 1); ros::Subscriber scan_sub = nh.subscribe("/scan", 1, scanCallback); ros::spin(); }
int main(int argc, char** argv) { ros::init(argc, argv, "filter_lidar"); ros::NodeHandle nh; ros::NodeHandle pNh("~"); igvc::getParam(pNh, "x_offset", x_offset); igvc::getParam(pNh, "y_offset", y_offset); igvc::getParam(pNh, "x_size", x_size); igvc::getParam(pNh, "y_size", y_size); igvc::getParam(pNh, "max_dist", max_dist); igvc::getParam(pNh, "back_buffer", back_buffer); _pointcloud_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ> >("/scan/pointcloud", 1); ros::Subscriber scan_sub = nh.subscribe("/pc2", 1, point_cloud_callback); ros::spin(); }
int main(int argc, char** argv) { ros::init(argc, argv, "pathplanner"); ros::NodeHandle nh; ros::Subscriber map_sub = nh.subscribe("/map", 1, map_callback); ros::Subscriber pose_sub = nh.subscribe("/odometry/filtered", 1, position_callback); ros::Subscriber waypoint_sub = nh.subscribe("/waypoint", 1, waypoint_callback); disp_path_pub = nh.advertise<nav_msgs::Path>("/path_display", 1); act_path_pub = nh.advertise<igvc_msgs::action_path>("/path", 1); expanded_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/expanded", 1); path_planner_map_pub = nh.advertise<pcl::PointCloud<pcl::PointXYZ>>("/path_planner_incremental", 1); double baseline = 0.93; ros::NodeHandle pNh("~"); search_problem.Map = pcl::PointCloud<pcl::PointXYZ>().makeShared(); search_problem.Map->header.frame_id = "/odom"; search_problem.Octree = boost::make_shared<pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>>(0.1); search_problem.Octree->setInputCloud(search_problem.Map); if (!pNh.hasParam("goal_threshold") || !pNh.hasParam("threshold") || !pNh.hasParam("speed") || !pNh.hasParam("baseline") || !pNh.hasParam("minimum_omega") || !pNh.hasParam("maximum_omega") || !pNh.hasParam("delta_omega") || !pNh.hasParam("point_turns_enabled") || !pNh.hasParam("reverse_enabled") || !pNh.hasParam("max_obstacle_delta_t") || !pNh.hasParam("alpha") || !pNh.hasParam("beta") || !pNh.hasParam("bounding_distance")) { ROS_ERROR_STREAM("path planner does not have all required parameters"); return 0; } pNh.getParam("goal_threshold", search_problem.GoalThreshold); pNh.getParam("threshold", search_problem.Threshold); pNh.getParam("speed", search_problem.Speed); pNh.getParam("baseline", search_problem.Baseline); search_problem.DeltaT = [](double distToStart, double distToGoal) -> double { return -((distToStart + distToGoal) / 7 / (pow((distToStart + distToGoal) / 2, 2)) * pow(distToStart - (distToStart + distToGoal) / 2, 2)) + (distToStart + distToGoal) / 7 + 0.3; }; pNh.getParam("minimum_omega", search_problem.MinimumOmega); pNh.getParam("maximum_omega", search_problem.MaximumOmega); pNh.getParam("delta_omega", search_problem.DeltaOmega); pNh.getParam("point_turns_enabled", search_problem.PointTurnsEnabled); pNh.getParam("reverse_enabled", search_problem.ReverseEnabled); pNh.getParam("max_obstacle_delta_t", search_problem.MaxObstacleDeltaT); pNh.getParam("alpha", search_problem.Alpha); pNh.getParam("beta", search_problem.Beta); pNh.getParam("bounding_distance", search_problem.BoundingDistance); ros::Rate rate(3); while (ros::ok()) { ros::spinOnce(); /* Do not attempt to plan a path if the path length would be greater than 100ft (~30m). * This should only happen when we have received either a waypoint or position estimate, but not both. * Long paths take forever to compute, and will freeze up this node. */ auto distance_to_goal = search_problem.Start.distTo(search_problem.Goal); if (!received_waypoint || distance_to_goal == 0 || distance_to_goal > 60) continue; planning_mutex.lock(); Path<SearchLocation, SearchMove> path; path = GraphSearch::AStar(search_problem, expanded_callback); if (act_path_pub.getNumSubscribers() > 0) { nav_msgs::Path disp_path_msg; disp_path_msg.header.stamp = ros::Time::now(); disp_path_msg.header.frame_id = "odom"; if (path.getStates()->empty()) path.getStates()->push_back(search_problem.Start); for (auto loc : *(path.getStates())) { geometry_msgs::PoseStamped pose; pose.header.stamp = disp_path_msg.header.stamp; pose.header.frame_id = disp_path_msg.header.frame_id; pose.pose.position.x = loc.x; pose.pose.position.y = loc.y; disp_path_msg.poses.push_back(pose); } disp_path_pub.publish(disp_path_msg); igvc_msgs::action_path act_path_msg; act_path_msg.header.stamp = ros::Time::now(); act_path_msg.header.frame_id = "odom"; for (auto action : *(path.getActions())) { igvc_msgs::velocity_pair vels; vels.header.stamp = act_path_msg.header.stamp; vels.header.frame_id = act_path_msg.header.frame_id; if (action.W != 0) { double radius = action.V / action.W; vels.right_velocity = (radius - baseline / 2.) * action.W; vels.left_velocity = (radius + baseline / 2.) * action.W; } else { vels.right_velocity = 1.0; vels.left_velocity = 1.0; } vels.duration = action.DeltaT; act_path_msg.actions.push_back(vels); } act_path_pub.publish(act_path_msg); } planning_mutex.unlock(); rate.sleep(); } return 0; }
int main() { // Setup ROS Arguments char** argv = NULL; int argc = 0; // Init ROS Node ros::init (argc, argv, "MR_Button"); ros::NodeHandle nh; ros::NodeHandle pNh ("~"); // Topic names std::string mrMainRunSrv, buttonSub, obstacleDetectorSub, mrMainModeSub; pNh.param<std::string> ("mr_button_sub", buttonSub, "/mrButton/status"); pNh.param<std::string> ("mr_obstacle_detector", obstacleDetectorSub, "/mrObstacleDetector/status"); pNh.param<std::string> ("mr_main_mode_sub", mrMainModeSub, "/mrMain/mode"); pNh.param<std::string> ("mr_main_run_srv", mrMainRunSrv, "/mrMain/run"); // Service _srvMrMainRun = nh.serviceClient<mr_main::run>(mrMainRunSrv); // Subscriber ros::Subscriber subButton = nh.subscribe (buttonSub, 1, buttonCallback); ros::Subscriber subObstacleDetector = nh.subscribe (obstacleDetectorSub, 1, obstacleDetectorCallback); ros::Subscriber subMrMainMode = nh.subscribe (mrMainModeSub, 1, mrMainModeCallback); // Get serial data parameters int baudRate; std::string port; pNh.param<int> ("baud_rate", baudRate, 115200); pNh.param<std::string> ("port", port, "/dev/serial/by-id/usb-Texas_Instruments_In-Circuit_Debug_Interface_0E203B83-if00"); // Open connection _serialConnection = new serial::Serial (port.c_str(), baudRate, serial::Timeout::simpleTimeout (50)); // Check if connection is ok if (!_serialConnection->isOpen()) { ROS_ERROR ("Error opening connection!"); _serialConnection->close(); return 0; } else ROS_INFO ("Successfully connected!"); // Start serial threads boost::thread readThread(readSerialThread); boost::thread writeThread(writeSerialThread); // Sleep for a second ros::Duration(2).sleep(); // Change mode to idle changeRunMode (M_IDLE); // ROS Spin: Handle callbacks while (!ros::isShuttingDown()) ros::spin(); // Close connection changeRunMode (M_OFF); ros::Duration(2).sleep(); readThread.interrupt(); writeThread.interrupt(); _serialConnection->close(); // Return return 0; }
int main() { // Setup ROS Arguments char** argv = NULL; int argc = 0; // Init ROS Node ros::init(argc, argv, "RC_Main"); ros::NodeHandle nh; ros::NodeHandle pNh("~"); // Topic names std::string hmiConsolePub, grabService, getBricksService, plcService, anyBricksSub, safetySub, mesPub, mesSub, hmiStatusSub; pNh.param<std::string>("hmiConsole", hmiConsolePub, "/rcHMI/console"); pNh.param<std::string>("grabService", grabService, "/rcGrasp/grabBrick"); pNh.param<std::string>("getBricksService", getBricksService, "/rcVision/getBricks"); pNh.param<std::string>("plcService", plcService, "/rcPLC"); pNh.param<std::string>("anyBricks_sub", anyBricksSub, "/rcVision/anyBricks"); pNh.param<std::string>("hmi_status_sub", hmiStatusSub, "/rcHMI/status"); pNh.param<std::string>("mesPub", mesPub, "/rcMESClient/msgToServer"); pNh.param<std::string>("mesSub", mesSub, "/rcMESClient/msgFromServer"); // Create service calls _serviceGrabBrick = nh.serviceClient<rc_grasp::grabBrick>(grabService); _serviceGetBricks = nh.serviceClient<rc_vision::getBricks>(getBricksService); _serviceMove = nh.serviceClient<rc_plc::MoveConv>(plcService + "/MoveConv"); _serviceStart = nh.serviceClient<rc_plc::StartConv>(plcService + "/StartConv"); _serviceStop = nh.serviceClient<rc_plc::StopConv>(plcService + "/StopConv"); _serviceChangeDir = nh.serviceClient<rc_plc::ChangeDirection>(plcService + "/ChangeDirection"); _serviceGetIsMoving = nh.serviceClient<kuka_rsi::getIsMoving>("/KukaNode/IsMoving"); _serviceGetConf = nh.serviceClient<kuka_rsi::getConfiguration>("/KukaNode/GetConfiguration"); _serviceGetSafety = nh.serviceClient<kuka_rsi::getSafety>("/KukaNode/GetSafety"); // Publishers _hmiConsolePub = nh.advertise<std_msgs::String>(hmiConsolePub, 100); _mesMessagePub = nh.advertise<std_msgs::String>(mesPub, 100); _mainStatusPub = nh.advertise<std_msgs::String>("/rcMain/status", 100); // Subscribers ros::Subscriber anyBrickSub = nh.subscribe(anyBricksSub, 10, anyBrickCallback); ros::Subscriber mesMessageSub = nh.subscribe(mesSub, 10, mesRecCallback); ros::Subscriber hmiStatusSubs = nh.subscribe(hmiStatusSub, 10, hmiStatusCallback); // Main handler thread boost::thread mainThread(mainHandlerThread); // Spin rate ros::Rate r(10); // 10 hz // Spin while(ros::ok()) { // Get safety kuka_rsi::getSafety obj; _serviceGetSafety.call(obj); _safetyMutex.lock(); _safety = obj.response.safetyBreached; _safetyMutex.unlock(); ros::spinOnce(); r.sleep(); } // Return mainThread.interrupt(); return 0; }