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;
}
