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