Seekur Software ( Ubuntu 12.04) - ITVRoC/SeekurJr GitHub Wiki
SeekurJr connection
Setting the route through the Seekur jr IP:
$ sudo ip route add 10.0.125.0/24 via 10.0.126.1
Comunication
Login to the server :
$ ssh [email protected]
password: mobilerobots
Packages
To work with SEEKUR JR within the ROS system, it was necessary to search for packages with drivers that offer control over the platform, Schunk arm and other devices (Stereo and PTZ Cameras, GPS, Laser, Bumpers, IMU)
The main packages are:
- Rosaria/Ros-ARNL: packets that interface with the Aria and Arnl system to communicate with SEEKUR JR.
- p2os: Package to control the engines of the mobile platform through speed commands. It provides interface for teleoperation through keyboard or joystick.
- schunk robotics: Driver and Schunk arm controller. The controller acts on the joint space using spline interpolation.
- MoveIt!: Plan trajectory with detour of obstacles and sends to the Schunk arm controller. Provides graphical interface and a variety of obstacle trajectory path planning algorithms.
- LMS1xx: Reads the angle and distance values provided by the laser LMS111.
- gscam: Displays the images captured by the PTZ .
- mobile_ranger: Stereo camera driver that uses the OpenCV library to display disparate images and calculate the depth of the selected points. In addition, an algorithm for detecting circles and ellipses was incorporated into this package.
- seekurjr_description: description of Seekur Jr in URDF format(Unified Robot Description Format)
- GMapping: Package to map the environment and locate the Seekur Jr on the map using SLAM method (Simultaneous Localization and Mapping).
- move_base: Planning and control of trajectories for autonomous navigation with detour of obstacles.
The ROS workspace (catkin_ws / src) is divided into folders according to the functionalities of the packages: Navigation / Mapping, Schunk Arm, Moveit, URDF, Camera_stereo, PTZ Camera, GPS and ROSARY / ROSARNL.
Packages can be executed directly using the command: rosrun package_name node_name or through launch files roslaunch package_name launch_name.launch
OBS Due to the mobile robots site being off the air, it is possible to access it only by cache:Main page ou categories
Running the packages
ARIA INTERFACE (ROSARIA/ROSARNL)
$ rosrun rosaria RosAria _port:=/dev/ttyS0
Description: In the ROSARIA package are created topics that demonstrate the status of some SEEKUR JR devices such as bumpers and battery.For more information Aria Manual
$ rosrun rosarnl rosarnl_node
Description: In the ROS-ARNL package topics are created to help in the mapping and navigation of SEEKUR JR through values of odometry and laser reading.
OBS: The ARIA (rosaria / rosarnl) system interface packages use the ttyS0 serial port. This port is also used by the P2OS_Driver package. Therefore, for compatibility reasons, ARIA system packages are not widely used.
LASER SICK (LMS1XX)
$ rosrun lms1xx LMS1xx_node _host:=192.168.0.1
Description: This node is the laser communication driver that creates the topic / scan that receives the laser measurements in the sensor_msgs / LaserScan message format.
MAPPING AND NAVIGATION (P2OS)
Mapping and navigation operations need the transformations (/ tf) between the platform components to function. When these packets are run on another machine, there must be clock synchronization between them so that the packets are found at the correct time. For this, the NTP server can be used. SEEKUR JR is configured (/etc/ntp.conf) to be an NTP server client. Thus, by configuring the other machine as a server (/etc/ntp.conf), you can use the command:
sudo ntpdate -u "remote_pc_IP"
This command must be executed on the SEEKUR JR terminal so that it synchronizes with the other machine.
$ roslaunch p2os_launch pioneerMasterLaunchFile.launch
Description: This launch file consists of several launch files. These launchs basically execute nodes that enable the teleoperation of SEEKUR JR and future mapping of the environment, as described below: p2os_driver: communicates with SEEKUR JR engines and IMU. Generates the base_link → Odom transform. teleop_keyboard: enables teleoperation by keyboard. teleop_joy: enables teleoperation by joystick. sicklms: calls the communication node with the laser described previously (LMS1XX). tf_base_link_to_laser: Generates a static transform between the base_link and the laser.
$ roslaunch p2os_launch gmapping.launch
Description: This launch executes the mapping node that uses the SLAM technique. The node receives the laser read values through the / scan topic and then generates a map of the environment in the / map topic whose message type is nav_msgs / OccupancyGrid. This package generates the transformed map → odom, which, with the previously generated transforms (odom → base_link → laser), makes it possible to calculate the location of SEEKUR JR within the generated map. The location of SEEKUR JR is published in the / pose topic.
$ roslaunch p2os_launch navigation.launch
Descrição: This launch consists of other launch files. These launchs are basically settings so that the move_base package performs navigation tasks with obstacle bypass. Before running this launch, you must first run gmapping because the navigation needs a map to work. When executed, navigation generates a local and global cost map. The RVIZ (rosrun rviz rviz) must be used both to visualize the maps and to indicate the trajectory target point. In RVIZ the target point is indicated by the 2D Nav Goal option. By choosing the endpoint where SEEKUR JR should be, the controller will (local_local_base) generate a global and local trajectory. The overall trajectory is directed towards the end point chosen. The local trajectory is directed towards the diversion of obstacles in the regions near SEEKUR JR. Both paths can also be viewed in RVIZ.
Obs: To verify in real time all navigation parameters configured in the move_base, you can use the command "rosrun rqt_reconfigure rqt_reconfigure". Through this tool it is possible to more quickly verify the impact of each parameter on the performance of the robot.
SCHUNK ARM
To move the SCHUNK arm you should first check if the doors (8 and 29) of the arm have been activated. To activate the ports:
$ seekurPower -8 reset -29 reset
$ roslaunch schunk_bringup lwa4p_solo.launch
Description: This launch executes the schunk arm driver that communicates through the CAN protocol. Along with the driver is created an action server of type FollowJointTrajectory that serves as interface to communicate with the arm controller. When sending arm position commands to the controller (via the topic: FollowJointTrajectory / goal), it uses the spline method to interpolate points and generate a trajectory. Sending commands to the arm controller can be done through an interface that comes along with the package called Dashboard or through the Moveit package.
$ roslaunch schunk_bringup dashboard_lwa4p.launch
Description: Interface to send commands to the schunk arm controller. Once the arm driver has been started, you must use the Init All command to initialize all nodes in the arm (usually the boot fails on the first attempt, so you must restart the lwa4p_solo driver and then try the command again)
Obs: In addition to triggering the arm via ROS it is possible to trigger it directly through the IPA_CANOPEN library located in the schunk_robots package folder. In the ipa_canopen / ipa_canopen_core / bin folder there are executable files that allow you to move and calibrate the arm nodes. Among the necessary parameters it is worth mentioning that the arm is located in / dev / pcan33 and that the communication works only with the PeakCan converter.
MOVEIT
Obs: To execute the moveit first you need to run the driver / controller arm and activate the joints of the handler through the init all command on the dashboard.
Moveit_seekur
The moveit_seekur package was generated through the moveit assistant (rosrun moveit_setup_assistant moveit_setup_assistant). To generate this package in the wizard, the file urdf.xacro of the SEEKUR JR equipped with the schunk arm was loaded.
The built-in Unified Robot Description Format (URDF) file is based on the seekurjr_gazebo and schunk_description description packages. It describes the main components of the robot and is essential for the moveit to calculate the workspace in which it can act.
$ roslaunch seekur_jr_moveit demo.launch
Description: Loads the generated controller, conflict matrix, and URDF parameters. Run the moveit package and open a GUI in RVIZ. Through the interface it is possible to adjust the initial and desired position of the arm, to plan the trajectory and to execute the planned movement. In addition, it is possible to insert objects in the manipulator environment to check trajectories with obstacle bypass.
Moveit_interface_seekur
The moveit_interface must be run after demo.launch. This package was created to send requests to the moveit through a node within the ROS. The package consists of 2 nodes.
$ rosrun moveit_interface_seekur move_group_interface_seekur
Description: This node integrates the identification of ellipses with the manipulation of the Schunk arm. To do this, it receives the position and depth values sent by camera_stereo (mobile_ranger) and then calculates the end position of the arm effector. Using the current position and calculated final position a path planning request is sent to the moveit. If a trajectory is found, it will be executed.
$ rosrun moveit_interface_seekur move_group_interface_integrado
Description: This knot was created to integrate the platform navigation system, the identification of holes and the manipulation of the schunk arm. When executed this node is waiting for a signal from the move_base that the navigation was completed successfully. After the positive sign of navigation the executed algorithm is similar to the one of the previous node.
CAMERA PTZ
Before running the PTZ node it is necessary to configure v4l2 for the NTSC format, through the command:
$ v4l2-ctl -s ntsc
In addition, it is necessary to check if port 10 of the SEEKUR JR is activated. To activate the port:
$ seekurPower -10 reset
Launcher for capturing images:
$ roslaunch gscam camera_raw.launch
Description: Publishes the images of the PTZ camera in a topic of type sensor_msgs / Image. Images can be viewed in the window that opens when executing the node or through RVIZ.
Node for PTZ control:
To control the PTZ camera by the ROS, you must first start the RosAria:
$ rosrun rosaria RosAria _port:=/dev/ttyS0
Then you must start the Aria interface node:
$ rosrun pan_tilt_zoom ptz_control.launch
Description: This node uses the ARIA interface to send pan, tilt, and zoom commands to the camera. The node subscribes to the topic '/ ptz_goal' and when a message arrives at the topic, it calls a callback that sends the command direct to Aria, controlling the PTZ. It also allows you to use the keyboard to move the camera. However using this method, no messages will be published in the topic. Commands are sent directly to the serial port.
CAMERA STEREO (MOBILE_RANGER)
The MobileRanger stereo camera has its own nDepth processing card whose eagle driver creates a device in the / dev / eagle0 directory. A "mobile_ranger" package was created that uses a code based on the OPENCV library that came with the factory SEEKUR JR.
$ rosrun mobile_ranger stereo_cam
Description: Based on the opencv_simple_color code. This node publishes in 3 topics. The / stereo_cam_right topic displays the raw images from the right camera, the / stereo_cam_disp_nocolor topic displays the disparate images, and the / stereo_cam_disp topic displays the disparate image where the depth is differentiated by color.
rosrun mobile_ranger stereo_cam_distance
Description: Based on the opencv_simple_color_mouse code and the fast_ellipse_detector ellipsis detection code. This node in addition to publishing the topics of the previous node, it publishes the coordinates and depth of the image in the center of the detected circle or ellipse. The depth and coordinates are calculated through a moving average with a 5-value buffer. Average values are published in the / depth topic.
GPS
To execute the node:
$ rosrun nmea_gps_driver nmea_gps_driver.py
Obs: The GPS needs a minimum of satellites to calculate its position. Inside the ITV lab this number is not reached and GPS does not return correct values. GPS has an application for windows (AgRemote) that generates a diagnosis in more detail. For more information about the GPS configuration see Seekur hardware - GPS