TTErr TTData::setGenericValue(const TTValue& value)
{
if (!mIsSending && mActive) {
// lock
mIsSending = YES;
// don't update the internal value with empty value
if (value.size()) {
// set internal value
mValue = value;
// clip the internal value
clipValue();
}
// return the internal value
returnValue();
// unlock
mIsSending = NO;
return kTTErrNone;
}
return kTTErrGeneric;
}
void pointCloudCallback(const sensor_msgs::PointCloud2& msg)
{
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg (msg, cloud);
/// Update map with each point
// Store robot position in map grid coordinates
unsigned int robot_col = cvRound((MAP_WIDTH/2+robot_pose.x)/MAP_RESOLUTION);
unsigned int robot_row = cvRound((MAP_HEIGHT/2-robot_pose.y)/MAP_RESOLUTION);
for( uint i=0; i < cloud.width; i++ )
{
// Get point in map grid coordinates
unsigned int obstacle_col = cvRound((MAP_WIDTH/2+cloud.points[i].x)/MAP_RESOLUTION);
unsigned int obstacle_row = cvRound((MAP_HEIGHT/2-cloud.points[i].y)/MAP_RESOLUTION);
// Get line iterator from robot to point
cv::LineIterator it(map,
cv::Point(robot_col, robot_row), // Start point
cv::Point(obstacle_col, obstacle_row), // End point
4); // Connectivity
// Update free space
for(int j=0; j < it.count-1; j++, ++it)
**it = clipValue(**it+CELL_DELTA_UP,MIN_CELL_VALUE, MAX_CELL_VALUE);
// Update occupied space
**it = clipValue(**it-CELL_DELTA_DOWN,MIN_CELL_VALUE, MAX_CELL_VALUE);
}
// Show map
cv::imshow("Mapa", map);
// Save the map every DELTA_SAVE iterations
iteration++;
if( iteration == DELTA_SAVE )
{
cv::imwrite("mapa.png", map);
iteration = 0;
}
return;
}
void laserCallback(const sensor_msgs::LaserScan& msg)
{
double angle;
unsigned int i;
/// Update distance to closest front obstacles
angle = -0.785; // DEG2RAD(-45)
i = round((angle - msg.angle_min)/msg.angle_increment);
double closest_front_obstacle = msg.range_max;
while( angle < 0.785 ) // DEG2RAD(45)
{
if( (msg.ranges[i] < msg.range_max) &&
(msg.ranges[i] > msg.range_min) &&
(msg.ranges[i] < closest_front_obstacle) )
closest_front_obstacle = msg.ranges[i];
i++;
angle += msg.angle_increment;
}
/// Perform navigation base on the detected obstacle
bool avoid = false;
if( closest_front_obstacle < min_front_dist )
{
if( closest_front_obstacle < stop_front_dist )
{
avoid = true;
lin_vel = -0.100;
} else
{
avoid = true;
lin_vel = 0;
}
} else
{
lin_vel = 0.5;
ang_vel = 0.0;
rotating = false;
}
if(avoid)
{
lin_vel = 0.0;
if( rotating == false )
{
// Favor rotation in the same direction as the last rotation
if( rng.uniform(0.0, 1.0) < 0.1 )
rotate_left = !rotate_left;
if( rotate_left == true )
ang_vel = DEG2RAD(30.0); // Rotate left
else
ang_vel = DEG2RAD(-30.0); // Rotate right
rotating = true;
}
}
// Limit maximum velocities
// (not needed here)
lin_vel = clipValue(lin_vel, -MAX_LIN_VEL, MAX_LIN_VEL);
ang_vel = clipValue(ang_vel, -MAX_ANG_VEL, MAX_ANG_VEL);
// Send velocity commands
vel_cmd.angular.z = ang_vel;
vel_cmd.linear.x = lin_vel;
vel_pub.publish(vel_cmd);
return;
}
TTErr TTData::setIntegerDecimalArrayValue(const TTValue& value)
{
// Storing locally to protect the input value
TTValue lValue = value;
if (!mIsSending && mActive)
{
// We are locking while updating, in order to prevent returned values from clients to cause an infinite loop.
mIsSending = YES;
if (checkIntegerDecimalArrayType(lValue))
{
if (lValue.size() > 0)
{
TTValue lPreviousValue = mValue;
// If ramp is performed using non-default dataspace unit, the returned values need to be converted.
if (mIsOverridingDataspaceUnit)
convertUnit(lValue, mValue);
else
mValue = lValue;
// Filter repetitions.
// This is done before clipping and integer truncation in order enforce views (returns) to stay within range when they attempt to set values out of range. See https://github.com/jamoma/JamomaMax/issues/934
// Additionally: If ramps reach the end prematurely (due to requests for ramp targets beyond the accepted range), the ramp will not be stopped.
if (mRepetitionsFilter)
{
if (mInitialized)
{
TTValue tempValue = mValue;
// Convert to integer if necsessary before comparing
if (mType == kTTSym_integer)
tempValue.truncate();
if (tempValue == lPreviousValue)
{
// Unlock and exit without outputing
mIsSending = NO;
return kTTErrNone;
}
}
}
// Clipping
clipValue();
// Truncate if type is integer
if (mType == kTTSym_integer)
mValue.truncate();
// Flag to ensure that value has been set at least once
mInitialized = true;
}
returnValue();
// unlock
mIsSending = NO;
return kTTErrNone;
}
else
{
// unlock
mIsSending = NO;
return kTTErrInvalidValue;
}
}
return kTTErrGeneric;
}
void TP8::laserCallback(const sensor_msgs::LaserScan& msg)
{
double angle, max_angle;
unsigned int i;
/// Update distance to closest front obstacles
angle = deg2rad(-45);
i = round((angle - msg.angle_min)/msg.angle_increment);
double closest_front_obstacle = msg.range_max;
max_angle = deg2rad(45);
while( angle < max_angle ) // DEG2RAD(45)
{
if( (msg.ranges[i] < msg.range_max) &&
(msg.ranges[i] > msg.range_min) &&
(msg.ranges[i] < closest_front_obstacle) )
closest_front_obstacle = msg.ranges[i];
i++;
angle += msg.angle_increment;
}
/// Perform navigation base on the detected obstacle
bool avoid = false;
if( closest_front_obstacle < _min_front_dist )
{
if( closest_front_obstacle < _stop_front_dist )
{
avoid = true;
_lin_vel = -0.100;
} else
{
avoid = true;
_lin_vel = 0;
}
} else
{
_lin_vel = 0.5;
_ang_vel = 0.0;
_rotating = false;
}
if(avoid)
{
_lin_vel = 0.0;
if( _rotating == false )
{
// Favor rotation in the same direction as the last rotation
if( _rng.uniform(0.0, 1.0) < 0.1 )
_rotate_left = !_rotate_left;
if( _rotate_left == true )
_ang_vel = deg2rad(30.0); // Rotate left
else
_ang_vel = deg2rad(-30.0); // Rotate right
_rotating = true;
}
}
// Limit maximum velocities
// (not needed here)
_lin_vel = clipValue(_lin_vel, -_MAX_LIN_VEL, _MAX_LIN_VEL);
_ang_vel = clipValue(_ang_vel, -_MAX_ANG_VEL, _MAX_ANG_VEL);
// Send velocity commands
_vel_cmd.angular.z = _ang_vel;
_vel_cmd.linear.x = _lin_vel;
_vel_pub.publish(_vel_cmd);
return;
}
