void handleRecievedData(char recv_buf[],Publisher config_pub,Publisher arm_pub)
{
std_msgs::String msg;
char type[4]= {'\0'};
strncpy(type,recv_buf,3);
if(!strcmp(type,"CAM"))
{
int written;
char temp[20]= {'\0'};
strncpy(temp,recv_buf+4,strlen(recv_buf));
ROS_INFO("sent data : %s",temp);
msg.data = temp;
config_pub.publish(msg);
}
else if(!strcmp(type,"ARM"))
{
int written = strlen(recv_buf);
char temp[20]= {'\0'};
strncpy(temp,recv_buf+4,written);
ROS_INFO("sent data : %s",temp);
msg.data = temp;
arm_pub.publish(msg);
}
else if(!strcmp(type,"PTZ"))
{
int written = strlen(recv_buf);
char temp[20]= {'\0'};
strncpy(temp,recv_buf+4,written);
ROS_INFO("sent data : %s",temp);
msg.data = temp;
ptz_pub.publish(msg);
}
}
void sensorPacketCallback(vector<float>& joints_pos, vector<float>& joints_adc, float pressure)
{
// Send joint state message
JointState msg_joints;
msg_joints.header.stamp = Time::now();
for(int i = 0; i < joints_adc.size(); i++)
{
msg_joints.name.push_back(joint_names[i]);
msg_joints.position.push_back(joints_adc[i]);
}
pub_joints.publish(msg_joints);
// Send pressure message
Float32 msg_pressure;
msg_pressure.data = pressure;
pub_pressure.publish(msg_pressure);
// Store current positions for IK computations
for(int i = 0; i < joints_pos.size(); i++)
{
q_current(i) = joints_pos[i];
}
}
void publishStatus(TimerEvent timerEvent) {
std_msgs::Int32 intMessage;
intMessage.data = _currentInput + 1;
_currentInputPublisher.publish(intMessage);
intMessage.data = _currentOutput + 1;
_currentOutputPublisher.publish(intMessage);
}
int main (int argc, char* argv[]) {
gengetopt_args_info args;
cmdline_parser(argc,argv,&args);
double v = args.linearVelocity_arg;
double a = args.angularVelocity_arg;
double time = args.time_arg;
int count = 0;
init(argc, argv, "talker");
geometry_msgs::Twist msg;
msg.linear.x = v;
msg.angular.z = a;
NodeHandle n;
Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi", 1);
ros::Rate loop_rate(10);
while (ros::ok() && count < (int)time*10) {
ros::spinOnce();
pub.publish(msg);
loop_rate.sleep();
count++;
}
for (int i = 0; i < 5; i++) {
msg.linear.x = 0;
msg.angular.z = 0;
pub.publish(msg);
ros::spinOnce();
}
return 0;
//init(argc, argv, "talker");
//geometry_msgs::Twist msg;
//msg.linear.x = 1.0;
//msg.angular.z = .5;
//int max_count = 30;
//int count = 0;
//NodeHandle n;
//Publisher pub = n.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi", 1);
//ros::Rate loop_rate(10);
//while (ros::ok() && count < max_count) {
// ros::spinOnce();
// pub.publish(msg);
// loop_rate.sleep();
// count++;
//}
//return 0;
}
void ImageCB(const sensor_msgs::ImageConstPtr& msg) {
cv_bridge::CvImagePtr cv_ptr;
Mat frame;
Mat output;
try {
cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
} catch (cv_bridge::Exception& e) {
ROS_ERROR("CV-Bridge error: %s", e.what());
return;
}
frame = cv_ptr->image;
frame = frame.mul(mask);
Mat white_lines = findWhiteLines(frame);
Mat blue_lines = findBlueLines(frame);
Mat yellow_lines = findYellowLines(frame);
Mat orange_blobs = findOrange(frame);
output = white_lines + blue_lines + yellow_lines + orange_blobs;
sensor_msgs::Image outmsg;
cv_ptr->image = output;
cv_ptr->encoding = "bgr8";
cv_ptr->toImageMsg(outmsg);
img_pub.publish(outmsg);
}
int main(int argc, char *argv[])
{
init(argc, argv, "position");
int key = 0;
NodeHandle n;
Position_Node::Position pos;
Rate loop_rate(1); /* in Hz */
Subscriber sub = n.subscribe("MarkerInfo", 1000, markerCallback);
Publisher pub = n.advertise<Position_Node::Position>("Position", 1000);
while(ros::ok())
{
/* Publish our position */
pos.x = g_robot_x;
pos.y = g_robot_y;
pos.theta = g_robot_theta;
//ROS_INFO("Published position.\n");
pub.publish(pos);
spinOnce();
loop_rate.sleep();
}
return 0;
}
void callback(const StereoPairIMUConstPtr& pair)
{
ImagePtr left(new Image), right(new Image);
PoseStampedPtr pose(new PoseStamped);
*left = pair->pair.leftImage;
*right = pair->pair.rightImage;
/**pose = pair->pose.quartenionPose;*/
*pose = pair->pose.eulerPose;
leftImagePub.publish(left);
rightImagePub.publish(right);
imuPosePub.publish(pose);
fpsp.print();
}
Benchmark()
: nh_()
, benchmark_state_sub_ (nh_.subscribe ("/roah_rsbb/benchmark/state", 1, &Benchmark::benchmark_state_callback, this))
, messages_saved_pub_ (nh_.advertise<std_msgs::UInt32> ("/roah_rsbb/messages_saved", 1, true))
{
// This should reflect the real number or size of messages saved.
std_msgs::UInt32 messages_saved_msg;
messages_saved_msg.data = 1;
messages_saved_pub_.publish (messages_saved_msg);
}
int main(int argc, char** argv){
ros::init(argc, argv, "command_module");
NodeHandle handler;
Subscriber s = handler.subscribe("command_message", 1000, messageHandle);
Subscriber q = handler.subscribe("odom", 1000, odomReceived);
Publisher p = handler.advertise<geometry_msgs::Twist>("cmd_vel", 1000);
ROS_INFO("** Command Moudule: On-line");
ROS_INFO("** Command Module: Waiting for Valid Commands");
Goal goal;
ros::Rate loop_rate(10);
while (ros::ok()) {
if (!GoalFifo.empty()) {
goal = GoalFifo.front();
ROS_INFO("** Command Module: Sending Goal");
if (goal.dist > 1.0) {
p.publish(move(0));
ros::Duration(goal.dist).sleep();
p.publish(stop());
}
if (goal.dTheta > 1.0) {
p.publish(turn(goal.dir));
ros::Duration(goal.dTheta).sleep();
p.publish(stop());
}
p.publish(stop());
GoalFifo.pop();
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
/**
* Controls all the inner workings of the PID functionality
* Should be called by _timer
*
* Controls the heating element Relays manually, overriding the standard relay
* functionality
*
* The pid is designed to Output an analog value, but the relay can only be On/Off.
*
* "time proportioning control" it's essentially a really slow version of PWM.
* first we decide on a window size. Then set the pid to adjust its output between 0 and that window size.
* lastly, we add some logic that translates the PID output into "Relay On Time" with the remainder of the
* window being "Relay Off Time"
*
* PID Adaptive Tuning
* You can change the tuning parameters. this can be
* helpful if we want the controller to be agressive at some
* times, and conservative at others.
*
*/
void Ohmbrewer::Thermostat::doPID(){
getSensor()->work();
setPoint = getTargetTemp()->c(); //targetTemp
input = getSensor()->getTemp()->c();//currentTemp
double gap = abs(setPoint-input); //distance away from target temp
//SET TUNING PARAMETERS
if (gap<10) { //we're close to targetTemp, use conservative tuning parameters
_thermPID->SetTunings(cons.kP(), cons.kI(), cons.kD());
}else {//we're far from targetTemp, use aggressive tuning parameters
_thermPID->SetTunings(agg.kP(), agg.kI(), agg.kD());
}
//COMPUTATIONS
_thermPID->Compute();
if (millis() - windowStartTime>windowSize) { //time to shift the Relay Window
windowStartTime += windowSize;
}
//TURN ON
if (getState() && gap!=0) {//if we want to turn on the element (thermostat is ON)
//TURN ON state and powerPin
if (!(getElement()->getState())) {//if heating element is off
getElement()->setState(true);//turn it on
if (getElement()->getPowerPin() != -1) { // if powerPin enabled
digitalWrite(getElement()->getPowerPin(), HIGH); //turn it on (only once each time you switch state)
}
}
//RELAY MODULATION
if (output < millis() - windowStartTime) {
digitalWrite(getElement()->getControlPin(), HIGH);
} else {
digitalWrite(getElement()->getControlPin(), LOW);
}
}
//TURN OFF
if (gap == 0 || getTargetTemp()->c() <= getSensor()->getTemp()->c() ) {//once reached target temp
getElement()->setState(false); //turn off element
getElement()->work();
// if (getElement()->getPowerPin() != -1) { // if powerPin enabled
// digitalWrite(getElement()->getPowerPin(), LOW); //turn it off too TODO check this
// }
// Notify Ohmbrewer that the target temperature has been reached.
Publisher pub = Publisher(new String(getStream()),
String("msg"),
String("Target Temperature Reached."));
pub.add(String("last_read_time"),
String(getSensor()->getLastReadTime()));
pub.add(String("temperature"),
String(getSensor()->getTemp()->c()));
pub.publish();
}
}
void transform_cloud(sensor_msgs::PointCloud2 cloud_in)
{
bool can_transform = listener.waitForTransform(cloud_in.header.frame_id,
frame_new,
ros::Time(0),
ros::Duration(3.0));
if (can_transform)
{
sensor_msgs::PointCloud2 cloud_out;
cloud_in.header.stamp = ros::Time(0);
pcl_ros::transformPointCloud(frame_new, cloud_in, cloud_out, listener);
pub.publish (cloud_out);
}
}
void publishGUI()
{
static double tLast = 0;
static double publishInterval = 0.016;
if(debugLevel<0 || GetTimeSec()-tLast<publishInterval)
return;
tLast = GetTimeSec();
ClearGUI();
//DrawMap();
guiMsg.robotLocX = curLoc.x;
guiMsg.robotLocY = curLoc.y;
guiMsg.robotAngle = curAngle;
localization->drawDisplay(guiMsg.lines_p1x, guiMsg.lines_p1y, guiMsg.lines_p2x, guiMsg.lines_p2y, guiMsg.lines_col, guiMsg.points_x, guiMsg.points_y, guiMsg.points_col, guiMsg.circles_x, guiMsg.circles_y, guiMsg.circles_col, 1.0);
//drawPointCloud();
guiPublisher.publish(guiMsg);
}
void operator()(const TimerEvent& t)
{
auv.updateAuv (storeforce);
geometry_msgs::Pose pos_msg;
kraken_msgs::imuData imu;
/*************************************/
pos_msg.position.x=auv._current_position_to_world[0];
pos_msg.position.y=auv._current_position_to_world[1];
pos_msg.position.z=auv._current_position_to_world[2];
//tf::Quaternion quat = tf::createQuaternionFromRPY(auv._current_position_to_world[3],auv._current_position_to_world[4],auv._current_position_to_world[5]);
tf::Quaternion quat = tf::createQuaternionFromRPY(auv._current_position_to_body[3],auv._current_position_to_body[4],auv._current_position_to_body[5]);
//tf::Quaternion quat = tf::createQuaternionFromRPY(/*auv._current_position_to_body[3]*/0,/*auv._current_position_to_body[4]*/0,/*auv._current_position_to_body[5]*/0.78);
// pos_msg.orientation= quat;
pos_msg.orientation.x=quat.getX ();
pos_msg.orientation.y=quat.getY ();
pos_msg.orientation.z=quat.getZ ();
pos_msg.orientation.w=quat.getW ();
/*************************************/
geometry_msgs::TwistStamped twist_msg;
twist_msg.twist.linear.x=auv._current_velocity_state_to_body[0];
twist_msg.twist.linear.y=auv._current_velocity_state_to_body[1];
twist_msg.twist.linear.z=auv._current_velocity_state_to_body[2];
imu.data[kraken_sensors::gyroX] = twist_msg.twist.angular.x=auv._current_velocity_state_to_body[3];
imu.data[kraken_sensors::gyroY]= twist_msg.twist.angular.y=auv._current_velocity_state_to_body[4];
imu.data[kraken_sensors::gyroZ] = twist_msg.twist.angular.z=auv._current_velocity_state_to_body[5];
twist_msg.header.frame_id='1';
/*************************************/
nav_msgs::Odometry odo_msg;
odo_msg.pose.pose=pos_msg;
odo_msg.twist.twist=twist_msg.twist;
/*************************************/
sensor_msgs::Imu imu_msg;
imu_msg.angular_velocity=twist_msg.twist.angular;
imu.data[kraken_sensors::accelX] = imu_msg.linear_acceleration.x=auv._current_accelaration_to_body[0];
imu.data[kraken_sensors::accelY] = imu_msg.linear_acceleration.y=auv._current_accelaration_to_body[1];
imu.data[kraken_sensors::accelZ] = imu_msg.linear_acceleration.z=auv._current_accelaration_to_body[2];
imu_msg.orientation=pos_msg.orientation;
//imu_pub.publish(imu_msg);
imu.data[kraken_sensors::roll] = auv._current_position_to_body[3];
imu.data[kraken_sensors::pitch] = auv._current_position_to_body[4];
imu.data[kraken_sensors::yaw] = auv._current_position_to_body[5];
imu_pub.publish(imu);
pose_pub.publish(pos_msg);
if(isLineNeeded)
odo_pub.publish(odo_msg);
}
void velCallback(const geometry_msgs::Twist::ConstPtr& msg) {
geometry_msgs::Twist twist;
if (msg->linear.x < 0)
twist.linear.x = 0;
else
twist.linear.x = new_vel*msg->linear.x;
twist.linear.y = msg->linear.y;
twist.linear.z = msg->linear.z;
twist.angular.x = msg->angular.x;
twist.angular.y = msg->angular.y;
twist.angular.z = msg->angular.z;
vel_pub.publish(twist);
}
int main( int argc, char* argv[])
{
init( argc, argv, "talker");
gengetopt_args_info args;
cmdline_parser( argc, argv, &args);
double v = args.linearVelocity_arg;
NodeHandle n;
Publisher pub = n.advertise<std_msgs::Empty>("/mobile_base/commands/reset_odometry", 1000);
//kobuki_msgs::Sound msg;
geometry_msgs::Twist msg;
//Publisher pub = n.advertise<msg_folder::msg_type>("topic name",1);
msg.linear.x =0;
double t = 10.0;
ros::Rate loop_rate(10);
int count = 0;
int max_count = t*10;
while( ros::ok() && count < max_count )
{
msg.angular.z = 1*sin(count/10.0);
pub.publish(msg);
ros:spinOnce();
loop_rate.sleep() ;
count++;
}
}
void pid_update() {
/* PID position update */
ctrl_data[0] = pid_vision_x -> update(img_center[1], dt); // OpenCV's x-axis is drone's y-axis
ctrl_data[1] = pid_vision_y -> update(img_center[0], dt);
//ctrl_data[2] = pid_z -> update(current_position.z, dt);
ctrl_data[3] = 0; //yaw
/* Control speed limiting */
window(&ctrl_data[0], visionLimit);
window(&ctrl_data[1], visionLimit);
window(&ctrl_data[2], visionLimit);
/* Publish the output control velocity from PID controller */
geometry_msgs::Vector3 velocity;
velocity.x = ctrl_data[0];
velocity.y = ctrl_data[1];
velocity.z = ctrl_data[2];
ctrl_vel_pub.publish(velocity);
}
int main(int argc, char **argv)
{
StereoRectification *rectification = new StereoRectification(EXTR_FILE, INTR_FILE, CAM_WIDTH, CAM_HEIGHT, WIDTH, HEIGHT);
StereoCamera *camera = new StereoCamera(rectification);
init(argc, argv, "StereoCapture");
NodeHandle nh;
Publisher pairPublisher = nh.advertise<StereoPair>("stereoCapture/stereoPair", 1);
StereoPairPtr pair;
Rate rate(FPS);
FPSPrinter fpsp;
while(ok())
{
camera->nextFrame();
pair->header.stamp = Time::now();
pair->leftImage = *camera->getLeftImage();
pair->rightImage = *camera->getRightImage();
pairPublisher.publish(pair);
spinOnce();
fpsp.print();
rate.sleep();
}
delete rectification;
delete camera;
return 0;
}
void publishLocation(bool limitRate)
{
static double tLast = 0;
if(GetTimeSec()-tLast<0.03 && limitRate)
return;
tLast = GetTimeSec();
LocalizationMsg msg;
localization->computeLocation(curLoc, curAngle);
msg.timeStamp = GetTimeSec();
msg.x = curLoc.x;
msg.y = curLoc.y;
msg.angle = curAngle;
msg.map = string(localization->getCurrentMapName());
localization->getUncertainty(msg.angleUncertainty, msg.locationUncertainty);
VectorLocalization2D::EvalValues laserEval, pointCloudEval;
localization->getEvalValues(laserEval,pointCloudEval);
msg.laserNumCorrespondences = laserEval.numCorrespondences;
msg.laserNumObservedPoints = laserEval.numObservedPoints;
msg.laserStage0Weights = laserEval.stage0Weights;
msg.laserStageRWeights = laserEval.stageRWeights;
msg.laserRunTime = laserEval.runTime;
msg.lastLaserRunTime = laserEval.lastRunTime;
msg.laserMeanSqError = laserEval.meanSqError;
msg.pointCloudNumCorrespondences = pointCloudEval.numCorrespondences;
msg.pointCloudNumObservedPoints = pointCloudEval.numObservedPoints;
msg.pointCloudStage0Weights = pointCloudEval.stage0Weights;
msg.pointCloudStageRWeights = pointCloudEval.stageRWeights;
msg.pointCloudRunTime = pointCloudEval.runTime;
msg.lastPointCloudRunTime = pointCloudEval.lastRunTime;
msg.pointCloudMeanSqError = pointCloudEval.meanSqError;
localizationPublisher.publish(msg);
//Publish particles
vector<Particle2D> particles;
localization->getParticles(particles);
particlesMsg.poses.resize(particles.size());
geometry_msgs::Pose particle;
for(unsigned int i=0; i<particles.size(); i++){
particle.position.x = particles[i].loc.x;
particle.position.y = particles[i].loc.y;
particle.position.z = 0;
particle.orientation.w = cos(0.5*particles[i].angle);
particle.orientation.x = 0;
particle.orientation.y = 0;
particle.orientation.z = sin(0.5*particles[i].angle);
particlesMsg.poses[i] = particle;
}
particlesPublisher.publish(particlesMsg);
//Publish map to base_footprint tf
try{
tf::StampedTransform odomToBaseTf;
transformListener->lookupTransform("odom","base_footprint",ros::Time(0), odomToBaseTf);
vector2f map_base_trans = curLoc;
float map_base_rot = curAngle;
vector2f odom_base_trans(odomToBaseTf.getOrigin().x(), odomToBaseTf.getOrigin().y());
float odom_base_rot = 2.0*atan2(odomToBaseTf.getRotation().z(), odomToBaseTf.getRotation().w());
vector2f base_odom_trans = -odom_base_trans.rotate(-odom_base_rot);
float base_odom_rot = -odom_base_rot;
vector2f map_odom_trans = map_base_trans + base_odom_trans.rotate(map_base_rot);
float map_odom_rot = angle_mod(map_base_rot + base_odom_rot);
tf::Transform mapToOdomTf;
mapToOdomTf.setOrigin(tf::Vector3(V2COMP(map_odom_trans), 0.0));
mapToOdomTf.setRotation(tf::Quaternion(tf::Vector3(0,0,1),map_odom_rot));
transformBroadcaster->sendTransform(tf::StampedTransform(mapToOdomTf, ros::Time::now(), "map", "odom"));
}
catch (tf::TransformException ex){
//Do nothing: We'll just try again next time
}
}
void gpsFixCallBack (geometry_msgs::PoseStamped gpsPose)
{
geometry_msgs::PoseStamped gpsFixPoseEstimate;
// find poseAtRequestTime
bool odomMovementFound = false;
pair<geometry_msgs::Pose, ros::Time> prevFront;
if (cb.size() > 0)
{
odomMovementFound = true;
cout << "odom found" << endl;
prevFront = cb.front();
if (gpsPose.header.stamp >= cb.front().second)
{
while (gpsPose.header.stamp > prevFront.second && cb.size() > 0)
{
prevFront = cb.front();
cb.pop_front();
}
}
}
// calculate Odom difference
double deltaX = 0;
double deltaY = 0;
if (odomMovementFound)
{
deltaX = (current.first.position.x - prevFront.first.position.x);
deltaY = (current.first.position.y - prevFront.first.position.y);
}
gpsFixPoseEstimate.pose.position.x = gpsPose.pose.position.x + deltaX;
gpsFixPoseEstimate.pose.position.y = gpsPose.pose.position.y + deltaY;
tf::Quaternion gpsQaut, odomCurrentQuat, odomOldQuat;
if (odomMovementFound)
{
tf::quaternionMsgToTF (current.first.orientation, odomCurrentQuat);
tf::quaternionMsgToTF (prevFront.first.orientation, odomOldQuat);
}
tf::quaternionMsgToTF (gpsPose.pose.orientation, gpsQaut);
double deltaYaw = 0;
if (odomMovementFound)
{
deltaYaw = (tf::getYaw (odomCurrentQuat) - tf::getYaw (odomOldQuat));
}
double newYaw = tf::getYaw (gpsQaut) + deltaYaw;
gpsFixPoseEstimate.pose.orientation = tf::createQuaternionMsgFromYaw (newYaw);
cout << "new Yaw: " << newYaw << endl;
// create covariance
// publish new estimated pose
gpsFixPoseEstimate.header.stamp = ros::Time::now();
gpsFixPoseEstimate.header.frame_id = gpsFrame;
geometry_msgs::PoseStamped inMapFrame;
try
{
tfListener->transformPose ("/map", ros::Time::now(), gpsFixPoseEstimate, gpsFrame, inMapFrame);
geometry_msgs::PoseWithCovarianceStamped resultPose;
resultPose.pose.pose = inMapFrame.pose;
resultPose.header = inMapFrame.header;
if (odomMovementFound)
{
resultPose.pose.covariance[0] = sqrt (abs (current.first.position.x - prevFront.first.position.x)); // X
resultPose.pose.covariance[7] = sqrt (abs (current.first.position.y - prevFront.first.position.y)); // Y
resultPose.pose.covariance[35] = sqrt (abs (newYaw)); // Yaw
}
else
{
resultPose.pose.covariance[0] = 0.1;// X
resultPose.pose.covariance[7] = 0.1; // Y
resultPose.pose.covariance[35] = 0.1; // Yaw
}
gpsOdomCombinedLocalisationPublisher.publish (resultPose);
}
catch (tf::TransformException ex)
{
ROS_ERROR ("%s", ex.what());
}
}
int main(int argc, char** argv)
{
// Initialize node and publisher.
init(argc, argv, "opencv_demo");
NodeHandle nodeHandle("~");
Publisher publisher = nodeHandle.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
const bool useTransparency = false;
// Create grid map.
GridMap map({"original", "elevation"});
map.setFrameId("map");
map.setGeometry(Length(1.2, 2.0), 0.01);
ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
map.getLength().x(), map.getLength().y(),
map.getSize()(0), map.getSize()(1));
// Add data.
if (!useTransparency) map["original"].setZero();
grid_map::Polygon polygon;
polygon.setFrameId(map.getFrameId());
polygon.addVertex(Position( 0.480, 0.000));
polygon.addVertex(Position( 0.164, 0.155));
polygon.addVertex(Position( 0.116, 0.500));
polygon.addVertex(Position(-0.133, 0.250));
polygon.addVertex(Position(-0.480, 0.399));
polygon.addVertex(Position(-0.316, 0.000));
polygon.addVertex(Position(-0.480, -0.399));
polygon.addVertex(Position(-0.133, -0.250));
polygon.addVertex(Position( 0.116, -0.500));
polygon.addVertex(Position( 0.164, -0.155));
polygon.addVertex(Position( 0.480, 0.000));
for (grid_map::PolygonIterator iterator(map, polygon); !iterator.isPastEnd(); ++iterator) {
map.at("original", *iterator) = 0.3;
}
// Convert to CV image.
cv::Mat originalImage;
if (useTransparency) {
// Note: The template parameters have to be set based on your encoding
// of the image. For 8-bit images use `unsigned char`.
GridMapCvConverter::toImage<unsigned short, 4>(map, "original", CV_16UC4, 0.0, 0.3, originalImage);
} else {
GridMapCvConverter::toImage<unsigned short, 1>(map, "original", CV_16UC1, 0.0, 0.3, originalImage);
}
// Create OpenCV window.
cv::namedWindow("OpenCV Demo");
// Work with copy of image in a loop.
while (nodeHandle.ok()) {
// Initialize.
ros::Time time = ros::Time::now();
map.setTimestamp(time.toNSec());
cv::Mat modifiedImage;
int blurRadius = 200 - abs((int)(200.0 * sin(time.toSec())));
blurRadius = blurRadius - (blurRadius % 2) + 1;
// Apply Gaussian blur.
cv::GaussianBlur(originalImage, modifiedImage, cv::Size(blurRadius, blurRadius), 0.0, 0.0);
// Visualize as image.
cv::imshow("OpenCV Demo", modifiedImage);
cv::waitKey(40);
// Convert resulting image to a grid map.
if (useTransparency) {
GridMapCvConverter::addLayerFromImage<unsigned short, 4>(modifiedImage, "elevation", map, 0.0, 0.3, 0.3);
} else {
GridMapCvConverter::addLayerFromImage<unsigned short, 1>(modifiedImage, "elevation", map, 0.0, 0.3);
}
// Publish grid map.
grid_map_msgs::GridMap message;
GridMapRosConverter::toMessage(map, message);
publisher.publish(message);
ROS_INFO_STREAM("Published image and grid map with blur radius " << blurRadius << ".");
}
return 0;
}
void ImageCB(const sensor_msgs::ImageConstPtr& msg) {
cv_bridge::CvImagePtr cv_ptr;
Mat frame;
Mat output;
try {
cv_ptr = cv_bridge::toCvCopy(msg, "bgr8");
} catch (cv_bridge::Exception& e) {
ROS_ERROR("CV-Bridge error: %s", e.what());
return;
}
frame = cv_ptr->image;
for(int r = 0; r < frame.rows; r++) {
unsigned char* row = frame.ptr<unsigned char>(r);
for(int c = 0; c < frame.cols * frame.channels(); c+= frame.channels()) {
auto& blue = row[c];
auto& green = row[c+1];
auto& red = row[c+2];
if(blue == 255 && green == 0 && red == 0) {
blue = 0;
} else if(blue != 0 || green != 0 || red != 0) {
blue = green = red = 255;
}
}
}
cvtColor(frame, frame, CV_BGR2GRAY);
//Circuit race steerer has more rectangles for sharper turns
auto leftRect = Rect(0,0,128,480);
auto leftCenterRect = Rect(128,0,128,480);
auto centerRect = Rect(256,0,128,480);
auto rightCenterRect = Rect(384,0,128,480);
auto rightRect = Rect(512,0,128,480);
auto left = frame(leftRect);
auto leftCenter = frame(leftCenterRect);
auto center = frame(centerRect);
auto rightCenter = frame(rightCenterRect);
auto right = frame(rightRect);
auto leftCount = countNonZero(left);
auto leftCenterCount = countNonZero(leftCenter);
auto centerCount = countNonZero(center);
auto rightCenterCount = countNonZero(rightCenter);
auto rightCount = countNonZero(right);
// Bias towards going straight
centerCount *= 0.90;
vector<int> counts = {leftCount, leftCenterCount, centerCount, rightCenterCount, rightCount};
auto min_index = distance(counts.begin(),min_element(counts.begin(),counts.end()));
rr_platform::steering steering_msg;
steering_msg.header.stamp = Time::now();
//circuit
switch(min_index) {
case 0: // left
rectangle(frame, leftRect, Scalar(255), 3);
steering_msg.angle = -30;
break;
case 1: // left center
rectangle(frame, leftCenterRect, Scalar(255), 3);
steering_msg.angle = -10;
break;
case 2: // center
rectangle(frame, centerRect, Scalar(255), 3);
steering_msg.angle = 0;
break;
case 3: // right center
rectangle(frame, rightCenterRect, Scalar(255), 3);
steering_msg.angle = 10;
break;
case 4: // right
rectangle(frame, rightRect, Scalar(255), 3);
steering_msg.angle = 30;
break;
}
if (speed == 0) {
steering_msg.angle = 0;
}
//cout << "Speed: " << speed;
steer_pub.publish(steering_msg);
sensor_msgs::Image outmsg;
cv_ptr->image = frame;
cv_ptr->encoding = "mono8";
cv_ptr->toImageMsg(outmsg);
img_pub.publish(outmsg);
}
int main(int argc, char* argv[]) {
gengetopt_args_info args;
cmdline_parser(argc,argv,&args);
float coeffs[6];
coeffs[0] = args.a1_arg;
coeffs[1] = args.a2_arg;
coeffs[2] = args.a3_arg;
coeffs[3] = args.a4_arg;
coeffs[4] = args.a5_arg;
coeffs[5] = args.a6_arg;
sigmaHit = args.sigma_arg;
angleMin = args.angleMin_arg;
angleMax = args.angleMax_arg;
angleMin *= (M_PI/180.0);
angleMax *= (M_PI/180.0);
angleIncrement = (angleMax-angleMin)/args.numBeams_arg;
numBeams = args.numBeams_arg;
float trueDistances[numBeams];
float noisyDistances[numBeams];
float commandReal[2];
init(argc, argv, "Odom");
nav_msgs::Odometry msg;
sensor_msgs::LaserScan msg2;
NodeHandle n;
Publisher pub = n.advertise<nav_msgs::Odometry>("odom", 1);
Publisher las = n.advertise<sensor_msgs::LaserScan>("/scan", 1);
Subscriber sub = n.subscribe("navi", 1000, cmmdUpdate); // update the command velocities
Subscriber rst = n.subscribe("/mobile_base/commands/reset_odometry", 1000, reset);
ros::Duration(1.3).sleep();
ros::Rate loop_rate(10);
while (ros::ok()) {
sampleMotionModel(command, commandReal, pose, coeffs, 0.1);
yawToQuaternion(pose[2], quaternion);
msg.pose.pose.position.x = pose[0];
msg.pose.pose.position.y = pose[1];
msg.pose.pose.orientation.w = quaternion[0];
msg.pose.pose.orientation.x = quaternion[1];
msg.pose.pose.orientation.y = quaternion[2];
msg.pose.pose.orientation.z = quaternion[3];
msg.twist.twist.linear.x = commandReal[0];
msg.twist.twist.angular.z = commandReal[1];
// Laser scanning section
calcTrueDistance(trueDistances, numBeams, angleIncrement);
calcNoisyDistance(noisyDistances, trueDistances, sigmaHit, numBeams);
msg2.angle_max = angleMax;
msg2.angle_min = angleMin;
msg2.time_increment = 0;
msg2.angle_increment = angleIncrement;
msg2.scan_time = 0.1;
msg2.range_min = 0;
msg2.range_max = maxRange;
msg2.ranges.resize(numBeams);
for (int i = 0; i < numBeams; i++) {
msg2.ranges[i] = noisyDistances[i];
}
ros::spinOnce();
pub.publish(msg);
las.publish(msg2);
loop_rate.sleep();
}
return 0;
}
/* this method is called whenever a new messegae is arrived */
void irCallBack(const sensor_msgs::LaserScanConstPtr& laserScanMsg)
{
//Variable assignment for ease of use
float angle_min = laserScanMsg->angle_min; //angle of IR sensor to right (rad)
float angle_max = laserScanMsg->angle_max; //angle of IR sensor to left (rad)
float angle_increment = laserScanMsg->angle_increment;
// double irMax = laserScanMsg->range_max; //maximum acceptable IR value
// double irMin = laserScanMsg->range_min; //minimum acceptable IR value
// std::cout << "# rays: " << (angle_max-angle_min)/(angle_increment) << std::endl;
/* Creating an imaginary wall passing through the cones*/
int point1 = ANGLE_MIN;
int point2 = ANGLE_MIN;
double myRanges[NUM_RAYS];
for (int i=0; i < NUM_RAYS; i++)
{
// Copying the distances into myRanges array
myRanges[i] = laserScanMsg->ranges[i];
if((myRanges[i] < 0.01 && myRanges[i] > -0.01)){// || myRanges[i] > 4.0){
myRanges[i] = MAX_DISTANCE;
}
/*
if ( i==7 )
std::cout << "myRanges[7]: " << myRanges[7] << std::endl;
if ( i==NUM_RAYS/2)
std::cout << "myRanges[NUM_RAYS/2]: " << myRanges[NUM_RAYS/2] << std::endl;
if ( i==NUM_RAYS-7)
std::cout << "myRanges[NUM_RAYS-7]: " << myRanges[NUM_RAYS-7] << std::endl;
*/
// If there is an empty space between the cones
if(myRanges[i] < FIELD || i == NUM_RAYS-1)
{
point2 = i;
for(int j = point2-1; point1 < j; j--)
{
myRanges[j] = (myRanges[point1]+myRanges[point2])/2;
}
point1 = point2+1;
}
}
double fxcomponents[NUM_RAYS];
double xcomponents[NUM_RAYS];
double dx[NUM_RAYS];
for(int i = 0; i < NUM_RAYS; i++)
{
if (i < NUM_RAYS/2)
{
//Geometry and Calculus skills!
fxcomponents[i] = myRanges[i] * cos((angle_min+ANGLE_MIN*angle_increment) + (i * angle_increment));
xcomponents[i] = myRanges[i] * sin((angle_min+ANGLE_MIN*angle_increment) + (i * angle_increment));
}
else
{
fxcomponents[i] = myRanges[i] * cos((i-NUM_RAYS/2) * angle_increment);
xcomponents[i] = myRanges[i] * sin((i-NUM_RAYS/2) * angle_increment);
}
}
/* Calculating the area enclosed by the rays */
double area = 0;
for(int i = 0; i < NUM_RAYS; i++)
{
if ( i == 0)
dx[i] = 0;
else
{
if (((xcomponents[i] > 0) & (xcomponents[i-1] >= 0)) || ((xcomponents[i] < 0) & (xcomponents[i-1] <= 0)))
dx[i] = abs(xcomponents[i] - xcomponents[i-1]);
else
dx[i] = abs(xcomponents[i] + xcomponents[i-1]);
}
area += (fxcomponents[i] * dx[i]);
}
/* Finding x-comp of center of mass */
double Gx = 0;
for (int i = 0; i < NUM_RAYS; i++)
{
Gx += ((xcomponents[i]*fxcomponents[i]*dx[i])/area);
}
/* Finding y-comp of center of mass */
double Gy = 0;
for (int i = 0; i < NUM_RAYS; i++)
{
Gy += ((fxcomponents[i]*fxcomponents[i]*dx[i])/area/2);
}
/*Finding the angle from the center to center of mass */
double theta = atan(Gy/Gx);
double thetaDeg = theta*180/M_PI;
double steering = calculateSteering(theta);
/* Reverse the steering if using hardware*/
// This is done because the sensor is physically up-side-down
// if (SIMULATOR_ON==0)
// steering = -steering;
std::cout << "Init steering: " << steering << std::endl;
double steeringBck;
double throttle = 0.5;
steering = lidar_ptr->sharpenSteering(steering);
/* Backing up */
for (int i=NUM_RAYS*4/10; i < NUM_RAYS*6/10; i++)
{
if (myRanges[i] < 0.6)
{
steeringBck = -steering;
throttle = -0.4;
dataPub.publish(lidar_ptr->vectorCalculator(throttle, steeringBck));
usleep(10000);
throttle = 0;
dataPub.publish(lidar_ptr->vectorCalculator(throttle, steeringBck));
usleep(10000);
throttle = -0.4;
dataPub.publish(lidar_ptr->vectorCalculator(throttle, steeringBck));
//ROS_INFO("I Published: throttle - %f, steering - %f", throttle, steering);
usleep(100000);
break;
}
}
dataPub.publish(lidar_ptr->vectorCalculator(throttle, steering));
ROS_INFO("I Published: throttle - %f, steering - %f", throttle, steering);
double irFront = myRanges[NUM_RAYS/2];//laserScanMsg->ranges[NUM_RAYS/2];
double irRight = myRanges[7];
double irLeft = myRanges[NUM_RAYS-7];
/* Some useful stuff to look after */
ROS_INFO("I received a new messege!\nfront: %f\nright: %f\nleft: %f\n", irFront, irRight, irLeft);
std::cout << "area: " << area << std::endl;
std::cout << "Center of mass (x,y): (" << Gx << ", " << Gy << ")" << std::endl;
std::cout << "theta (Gy/Gx): " << theta << std::endl;
std::cout << "thetaDEG: " << thetaDeg << std::endl;
std::cout << "Throttle: " << throttle << ", Steering: " << steering << std::endl;
std::cout << "*****************************\n" << std::endl;
}
/* if ( tcgetattr ( USB, &tty ) != 0 )
{
ROS_INFO("Error :%d , from tcgetattr:%s \n",errno,strerror(errno));
}
setParameters(tty,USB);
return USB;
}*/
void handleRecievedData(char recv_buf[],Publisher config_pub,Publisher arm_pub)
{
std_msgs::String msg;
char type[4]={'\0'};
strncpy(type,recv_buf,3);
if(!strcmp(type,"CAM"))
{
int written;
char temp[20]={'\0'};
strncpy(temp,recv_buf+4,strlen(recv_buf));
ROS_INFO("sent data : %s",temp);
msg.data = temp;
config_pub.publish(msg);
}
else if(!strcmp(type,"ARM"))
{
int written = strlen(recv_buf);
char temp[20]={'\0'};
strncpy(temp,recv_buf+4,written);
ROS_INFO("sent data : %s",temp);
msg.data = temp;
arm_pub.publish(msg);
}
else if(!strcmp(type,"RLY"))
{
int written = strlen(recv_buf);
char temp[20]={'\0'};
strncpy(temp,recv_buf+4,written);
msg.data = temp;
rly_pub.publish(msg);
}
else if(!strcmp(type,"DRV"))
{
int written = strlen(recv_buf);
char temp[20]={'\0'};
strncpy(temp,recv_buf+4,written);
msg.data = temp;
drv_pub.publish(msg);
}
else if(!strcmp(type,"CCM"))
{
int written = strlen(recv_buf);
char temp[20]={'\0'};
strncpy(temp,recv_buf+4,written);
msg.data = temp;
cam_pub.publish(msg);
}
else if(!strcmp(type,"PTZ"))
{
int written = strlen(recv_buf);
char temp[20]={'\0'};
int var_arr[7] = {0};int i=0;
strncpy(temp,recv_buf+4,written);
char *pch = strtok(temp,",");
while(pch != NULL)
{
var_arr[i++] = atoi(pch);
pch = strtok(NULL,",");
}
// Axis axis;
// axis.pan = (float)var_arr[0];axis.tilt = (float)var_arr[1];axis.zoom = (float)var_arr[2];
// axis.focus = (float)var_arr[3];axis.brightness = (float)var_arr[4];axis.autofocus =(bool)var_arr[6];
// ptz_pub.publish(axis);
}
}
int main(int argc, char** argv)
{
// Initialize node and publisher.
init(argc, argv, "move_demo");
NodeHandle nh("~");
Publisher publisher = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
// Create grid map.
GridMap map({"layer"});
map.setFrameId("map");
map.setGeometry(Length(0.7, 0.7), 0.01, Position(0.0, 0.0));
ROS_INFO("Created map with size %f x %f m (%i x %i cells).\n The center of the map is located at (%f, %f) in the %s frame.",
map.getLength().x(), map.getLength().y(),
map.getSize()(0), map.getSize()(1),
map.getPosition().x(), map.getPosition().y(), map.getFrameId().c_str());
map["layer"].setRandom();
bool useMoveMethod = true;
while (nh.ok()) {
if (useMoveMethod) {
ROS_INFO("Using the `move(...)` method.");
} else {
ROS_INFO("Using the `setPosition(...)` method.");
}
// Work with temporary map in a loop.
GridMap tempMap(map);
Rate rate(10.0);
ros::Time startTime = ros::Time::now();
ros::Duration duration(0.0);
while (duration <= ros::Duration(10.0)) {
ros::Time time = ros::Time::now();
duration = time - startTime;
// Change position of the map with either the `move` or `setPosition` method.
const double t = duration.toSec();
Position newPosition = 0.03 * t * Position(cos(t), sin(t));
if (useMoveMethod) {
tempMap.move(newPosition);
} else {
tempMap.setPosition(newPosition);
}
// Publish grid map.
tempMap.setTimestamp(time.toNSec());
grid_map_msgs::GridMap message;
GridMapRosConverter::toMessage(tempMap, message);
publisher.publish(message);
ROS_DEBUG("Grid map (duration %f) published with new position [%f, %f].",
duration.toSec(), tempMap.getPosition().x(), tempMap.getPosition().y());
rate.sleep();
}
useMoveMethod = !useMoveMethod;
}
return 0;
}
void depthCallback(const sensor_msgs::Image &msg)
{
//Copy the depth data
if(debugLevel>0){
printf("Depth Message t:%f\n", msg.header.stamp.toSec());
}
const uint8_t* ptrSrc = msg.data.data();
uint16_t depth[640*480];
memcpy(depth, ptrSrc, 640*480*(sizeof(uint16_t)));
if(!usePointCloud)
return;
tf::StampedTransform baseLinkToKinect;
try{
transformListener->lookupTransform("base_footprint", msg.header.frame_id, ros::Time(0), baseLinkToKinect);
}
catch(tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
tf::Vector3 translationTf = baseLinkToKinect.getOrigin();
tf::Quaternion rotationTf = baseLinkToKinect.getRotation();
Quaternionf rotQuat3D(rotationTf.w(), rotationTf.x(), rotationTf.y(), rotationTf.z());
Matrix3f rotMatrix(rotQuat3D);
kinectToRobotTransform.m11 = rotMatrix(0,0);
kinectToRobotTransform.m12 = rotMatrix(0,1);
kinectToRobotTransform.m13 = rotMatrix(0,2);
kinectToRobotTransform.m14 = translationTf.x();
kinectToRobotTransform.m21 = rotMatrix(1,0);
kinectToRobotTransform.m22 = rotMatrix(1,1);
kinectToRobotTransform.m23 = rotMatrix(1,2);
kinectToRobotTransform.m24 = translationTf.y();
kinectToRobotTransform.m31 = rotMatrix(2,0);
kinectToRobotTransform.m32 = rotMatrix(2,1);
kinectToRobotTransform.m33 = rotMatrix(2,2);
kinectToRobotTransform.m34 = translationTf.z();
kinectToRobotTransform.m41 = 0;
kinectToRobotTransform.m42 = 0;
kinectToRobotTransform.m43 = 0;
kinectToRobotTransform.m44 = 1;
//Generate filtered point cloud
vector<vector3f> filteredPointCloud;
vector<vector3f> pointCloudNormals;
vector<vector3f> outlierCloud;
vector<vector2i> pixelLocs;
vector<PlanePolygon> planePolygons;
planeFilter.GenerateFilteredPointCloud(depth, filteredPointCloud, pixelLocs, pointCloudNormals, outlierCloud, planePolygons);
//Transform from kinect coordinates to robot coordinates
for(unsigned int i=0; i<filteredPointCloud.size(); i++){
filteredPointCloud[i] = filteredPointCloud[i].transform(kinectToRobotTransform);
pointCloudNormals[i] = pointCloudNormals[i].transform(kinectToRobotTransform);
}
if(debugLevel>0){
filteredPointCloudMsg.points.clear();
filteredPointCloudMsg.header.stamp = ros::Time::now();
vector<geometry_msgs::Point32>* points = &(filteredPointCloudMsg.points);
geometry_msgs::Point32 p;
for(int i=0; i<(int)filteredPointCloud.size(); i++){
p.x = filteredPointCloud[i].x;
p.y = filteredPointCloud[i].y;
p.z = filteredPointCloud[i].z;
points->push_back(p);
}
filteredPointCloudPublisher.publish(filteredPointCloudMsg);
}
//Call particle filter
{
vector<vector2f> pointCloud2D, pointCloudNormals2D;
for(unsigned int i=0; i<filteredPointCloud.size(); i++){
if(fabs(pointCloudNormals[i].z)>sin(RAD(30.0)))
continue;
vector2f curNormal(V2COMP(pointCloudNormals[i]));
vector2f curPoint(V2COMP(filteredPointCloud[i]));
curNormal.normalize();
pointCloudNormals2D.push_back(curNormal);
pointCloud2D.push_back(curPoint);
}
localization->refinePointCloud(pointCloud2D, pointCloudNormals2D, pointCloudParams);
localization->updatePointCloud(pointCloud2D, pointCloudNormals2D, motionParams, pointCloudParams);
localization->resample(VectorLocalization2D::LowVarianceResampling);
localization->computeLocation(curLoc,curAngle);
}
}