Skip to content
Snippets Groups Projects
Commit a87d8246 authored by Christoph Schröter's avatar Christoph Schröter
Browse files

inital

parents
Branches
No related tags found
No related merge requests found
Expand all Show whitespace changes
Inline Side-by-side
Showing
with 1646 additions and 0 deletions
.idea/cobot_1.iml 0 → 100644
+ 3
− 0
View file @ a87d8246
<?xml version="1.0" encoding="UTF-8"?>
<module classpath="CMake" type="CPP_MODULE" version="4" />
\ No newline at end of file
.idea/misc.xml 0 → 100644
+ 5
− 0
View file @ a87d8246
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="CMakeWorkspace" PROJECT_DIR="$PROJECT_DIR$" />
</project>
\ No newline at end of file
.idea/modules.xml 0 → 100644
+ 9
− 0
View file @ a87d8246
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/cobot_1.iml" filepath="$PROJECT_DIR$/.idea/cobot_1.iml" />
</modules>
</component>
</project>
\ No newline at end of file
.idea/vcs.xml 0 → 100644
+ 8
− 0
View file @ a87d8246
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$/../.." vcs="Git" />
<mapping directory="$PROJECT_DIR$" vcs="Git" />
</component>
</project>
\ No newline at end of file
.idea/workspace.xml 0 → 100644
+ 0
− 0
View file @ a87d8246
This diff is collapsed.
CMakeLists.txt 0 → 100644
+ 75
− 0
View file @ a87d8246
cmake_minimum_required(VERSION 3.0.2)
project(cobot_1)
#add_compile_options(-std=c++14)
set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
# Find catkin macros and libraries if COMPONENTS list like `find_package(catkin REQUIRED COMPONENTS xyz)` is used,
# also find other catkin packages
find_package(catkin REQUIRED
COMPONENTS controller_manager
effort_controllers
gazebo_ros
genmsg
joint_state_controller
joint_state_publisher
joint_trajectory_controller
robot_state_publisher
roscpp
std_msgs
xacro
moveit_core
moveit_visual_tools
moveit_ros_planning
moveit_ros_planning_interface
controller_interface
hardware_interface
cooperation_msgs
pick_place
constraints
object_creator
panda_grasping
planning_util)
# System dependencies are found with CMake's conventions
find_package(PkgConfig REQUIRED)
# ######################################################################################################################
# catkin specific configuration ##
# ######################################################################################################################
# The catkin_package macro generates cmake config files for your package and declare things to be passed to dependent
# projects. This has to be done in addition to the find_package command above.
catkin_package(CATKIN_DEPENDS
moveit_core
moveit_visual_tools
moveit_ros_planning_interface
controller_interface
hardware_interface
pluginlib
DEPENDS
LIBRARIES ${PROJECT_NAME}_grasp_util
# system_lib
)
# ######################################################################################################################
# Build ##
# ######################################################################################################################
# Specify additional locations of header files Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR}
${CMAKE_CURRENT_SOURCE_DIR}
)
add_executable(CobotController src/CobotController.cpp src/Cobot.cpp )
add_executable(TRON_Adapter src/tron_adapter.cpp)
add_dependencies(CobotController ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Add cmake target dependencies of the executable
target_link_libraries(CobotController ${catkin_LIBRARIES})
target_link_libraries(TRON_Adapter ${catkin_LIBRARIES})
\ No newline at end of file
cobot1.xml 0 → 100644
+ 171
− 0
View file @ a87d8246
<?xml version="1.0" encoding="utf-8"?>
<!DOCTYPE nta PUBLIC '-//Uppaal Team//DTD Flat System 1.1//EN' 'http://www.it.uu.se/research/group/darts/uppaal/flat-1_2.dtd'>
<nta>
<declaration>// Place global declarations here.
int retry_count = 0;
int init_retry_count = 0;
chan pressed;
chan pickup_fail, pickup_success, move_fail, move_success;</declaration>
<template>
<name>press</name>
<location id="id0" x="-3153" y="-3026">
</location>
<location id="id1" x="-3017" y="-2983">
</location>
<init ref="id0"/>
<transition>
<source ref="id0"/>
<target ref="id1"/>
<label kind="synchronisation" x="-3119" y="-3034">pressed!</label>
</transition>
</template>
<template>
<name>move</name>
<location id="id2" x="-612" y="-68">
</location>
<init ref="id2"/>
<transition>
<source ref="id2"/>
<target ref="id2"/>
<label kind="synchronisation" x="-646" y="51">move_fail!</label>
<nail x="-663" y="42"/>
<nail x="-535" y="59"/>
</transition>
<transition>
<source ref="id2"/>
<target ref="id2"/>
<label kind="synchronisation" x="-653" y="-237">move_success!</label>
<nail x="-671" y="-229"/>
<nail x="-501" y="-212"/>
</transition>
</template>
<template>
<name>pickup</name>
<location id="id3" x="0" y="0">
</location>
<init ref="id3"/>
<transition>
<source ref="id3"/>
<target ref="id3"/>
<label kind="synchronisation" x="-34" y="102">pickup_fail!</label>
<nail x="-76" y="93"/>
<nail x="110" y="76"/>
</transition>
<transition>
<source ref="id3"/>
<target ref="id3"/>
<label kind="synchronisation" x="-109" y="-93">pickup_success!</label>
<nail x="-127" y="-153"/>
<nail x="136" y="-102"/>
</transition>
</template>
<template>
<name x="5" y="5">Cobot</name>
<declaration>// Place local declarations here.</declaration>
<location id="id4" x="-102" y="42">
<name x="-112" y="8">waiting</name>
</location>
<location id="id5" x="204" y="68">
<name x="170" y="85">pick_bottle</name>
</location>
<location id="id6" x="595" y="42">
<name x="544" y="59">move_to_glass</name>
</location>
<location id="id7" x="1020" y="59">
<name x="926" y="68">start_pour</name>
</location>
<location id="id8" x="442" y="-119">
<name x="399" y="-153">shut_down</name>
</location>
<location id="id9" x="918" y="289">
<name x="867" y="314">stop_pour</name>
</location>
<location id="id10" x="442" y="289">
<name x="399" y="314">place_bottle</name>
</location>
<init ref="id4"/>
<transition>
<source ref="id9"/>
<target ref="id10"/>
<label kind="synchronisation" x="595" y="263">move_success?</label>
</transition>
<transition>
<source ref="id9"/>
<target ref="id8"/>
<label kind="guard" x="1181" y="0">retry_count == 1</label>
<label kind="synchronisation" x="1232" y="-25">pickup_fail?</label>
<nail x="1436" y="-93"/>
</transition>
<transition>
<source ref="id9"/>
<target ref="id9"/>
<label kind="guard" x="1071" y="323">retry_count &gt; 1</label>
<label kind="synchronisation" x="1079" y="306">move_fail?</label>
<label kind="assignment" x="1062" y="340">retry_count--</label>
<nail x="1096" y="246"/>
<nail x="1037" y="382"/>
</transition>
<transition>
<source ref="id7"/>
<target ref="id9"/>
<label kind="synchronisation" x="884" y="102">move_success?</label>
<label kind="assignment" x="765" y="119">retry_count = init_retry_count</label>
</transition>
<transition>
<source ref="id7"/>
<target ref="id7"/>
<label kind="synchronisation" x="969" y="-42">move_fail?</label>
<nail x="918" y="-25"/>
<nail x="1139" y="-25"/>
</transition>
<transition>
<source ref="id6"/>
<target ref="id7"/>
<label kind="synchronisation" x="714" y="25">move_success?</label>
</transition>
<transition>
<source ref="id6"/>
<target ref="id6"/>
<label kind="synchronisation" x="544" y="-51">move_fail?</label>
<nail x="518" y="-8"/>
<nail x="637" y="-25"/>
</transition>
<transition>
<source ref="id5"/>
<target ref="id8"/>
<label kind="guard" x="280" y="-102">retry_count == 1</label>
<label kind="synchronisation" x="306" y="-119">pickup_fail?</label>
</transition>
<transition>
<source ref="id5"/>
<target ref="id6"/>
<label kind="synchronisation" x="365" y="25">pickup_success?</label>
</transition>
<transition>
<source ref="id5"/>
<target ref="id5"/>
<label kind="guard" x="110" y="-102">retry_count &gt; 1</label>
<label kind="synchronisation" x="110" y="-85">pickup_fail?</label>
<label kind="assignment" x="110" y="-68">retry_count--</label>
<nail x="127" y="-25"/>
<nail x="289" y="-42"/>
</transition>
<transition>
<source ref="id4"/>
<target ref="id5"/>
<label kind="select" x="-85" y="68">i : int[1,5]</label>
<label kind="synchronisation" x="-85" y="102">pressed?</label>
<label kind="assignment" x="-85" y="119">init_retry_count = i, retry_count = init_retry_count</label>
</transition>
</template>
<system>// Place template instantiations here.
// List one or more processes to be composed into a system.
system Cobot, press, move, pickup;
</system>
<queries>
<query>
<formula></formula>
<comment></comment>
</query>
</queries>
</nta>
config/cobot1_config.yaml 0 → 100644
+ 68
− 0
View file @ a87d8246
/object/table/width: 1
/object/table/depth: 3
/object/table/height: 0.1
/object/table/x: 0
/object/table/y: 0
/object/table/z: 0
/object/table/mass: 20
/object/table/is_static: true
/object/bottle/width: 0.05
/object/bottle/depth: 0.05
/object/bottle/height: 0.20
/object/bottle/x: 0.8
/object/bottle/y: 0.3
/object/bottle/z: 0.1005
/object/bottle/place/x: 0.8
/object/bottle/place/y: 0.3
/object/bottle/place/z: 0.1005
/object/bottle/mass: 0.7
/object/bottle/is_static: false
/object/glass/width: 0.05
/object/glass/depth: 0.05
/object/glass/height: 0.15
/object/glass/x: 0.7
/object/glass/y: 0
/object/glass/z: 0.106
/object/glass/place/x: -0.7
/object/glass/place/y: 0
/object/glass/place/z: 0.106
/object/glass/mass: 0.3
/object/glass/is_static: false
/object/pressure_sensor1/width: 0.2
/object/pressure_sensor1/depth: 0.2
/object/pressure_sensor1/height: 0.03
/object/pressure_sensor1/x: 0.7
/object/pressure_sensor1/y: 0
/object/pressure_sensor1/z: 0.015
/object/pressure_sensor1/mass: 0.5
/object/pressure_sensor1/is_static: true
/object/pressure_sensor2/width: 0.2
/object/pressure_sensor2/depth: 0.2
/object/pressure_sensor2/height: 0.03
/object/pressure_sensor2/x: -0.7
/object/pressure_sensor2/y: 0
/object/pressure_sensor2/z: 0.015
/object/pressure_sensor2/mass: 0.5
/object/pressure_sensor2/is_static: true
/object/safezone/width: 3
/object/safezone/depth: 0.55
/object/safezone/height: 3
/object/safezone/x: -0.65
/object/safezone/y: 0
/object/safezone/z: 0
/object/safezone/mass: 0
/object/safezone/is_static: true
/sensor/pressure/host: "localhost"
/sensor/pressure/port: 4223
/sensor/pressure/uid: "Gak"
/sensor/pressure/acivation_threshold: 200
/planning/retries: 5
/planning/time_factor: 2
\ No newline at end of file
include/Cobot.h 0 → 100644
+ 83
− 0
View file @ a87d8246
//
// Created by jim on 04.01.21.
//
#ifndef COBOT_1_COBOT_H
#define COBOT_1_COBOT_H
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include "Constraint.h"
class Cobot {
public:
// ctor
Cobot(moveit::planning_interface::MoveGroupInterface &group);
// member functions
/**
* Move the robot to its start position
* @param group the move_group on which the action shall be performed
*/
virtual bool moveToStart(moveit::planning_interface::MoveGroupInterface &group, bool with_constraints);
/**
* Perform a grasping action on the bottle
* @param group the move_group on which the action shall be performed
* @param bottle the collision object that represents the bottle
* @return true if the execution was successful, else false
*/
virtual bool pickBottle(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject bottle,
int retry_on_failure_count = 1, double retry_planning_time_factor = 1);
/**
* Fill the glass by moving the bottle closer to it and rotate the end effector to pour the liquid
* @param group the move_group on which the action shall be performed
* @param glass the glass to be filled
* @return true if the execution was successful, else false
*/
virtual bool fillGlass(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject glass,
int retry_on_failure_count = 1, double retry_planning_time_factor = 1);
/**
* Place the bottle back on its start position
* @param group the move_group on which the action shall be performed
* @param bottle the collision object that represents the bottle
* @return true if the execution was successful, else false
*/
virtual bool placeBottle(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject bottle,
int retry_on_failure_count = 1, double retry_planning_time_factor = 1);
/**
* Pick up the glass
* @param group the move_group on which the action shall be performed
* @param glass the collision object that represents the glass
* @return true if the execution was successful, else false
*/
virtual bool pickGlass(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject glass,
int retry_on_failure_count = 1, double retry_planning_time_factor = 1);
/**
* Place the glass on the position defined in the glass object
* @param group group the move_group on which the action shall be performed
* @param glass the collision object that represents the glass and holds the goal position
* @return true if the execution was successful, else false
*/
virtual bool placeGlass(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject glass,
geometry_msgs::Point target_place_position, int retry_on_failure_count = 1, double retry_planning_time_factor = 1);
private:
std::vector<double> start_state_joint_values;
moveit::planning_interface::MoveGroupInterface::Plan movePlan;
ProximityConstraint proximityConstraint;
OrientationConstraint orientationConstraint;
VelocityConstraint velocityConstraint;
AccelerationConstraint accelerationConstraint;
};
#endif //COBOT_1_COBOT_H
launch/cobot1.launch 0 → 100644
+ 21
− 0
View file @ a87d8246
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<include file="$(find safe_zone_controller)/launch/safe_zone_controller.launch" />
<include file="$(find pressure_sensor)/launch/pressure_sensor.launch" />
<node name="Cobot1_Controller" pkg="cobot_1" type="CobotController" respawn="false" output="screen">
<rosparam command="load" file="$(find cobot_1)/config/cobot1_config.yaml" />
</node>
<!-- Enforce joint space planning. Otherwise an orientation constraint would always result in cartesian planning -->
<param name="/move_group/panda_arm/enforce_joint_model_state_space" value="true"/>
<!-- The execution_duration_monitoring sometimes aborts a valid trajectory execution -->
<param name="move_group/trajectory_execution/execution_duration_monitoring" value="false" />
</launch>
launch/cobot1_minimal_mock.launch 0 → 100644
+ 22
− 0
View file @ a87d8246
<launch>
<!-- Enforce joint space planning. Otherwise an orientation constraint would always result in cartesian planning -->
<param name="/move_group/panda_arm/enforce_joint_model_state_space" value="true"/>
<!-- The execution_duration_monitoring sometimes aborts a valid trajectory execution -->
<param name="move_group/trajectory_execution/execution_duration_monitoring" value="false" />
<include file="$(find panda_simulation)/launch/simulation.launch">
<arg name="slow_simulation" value="false" />
<arg name="gui" value="false" />
</include>
<include file="$(find safe_zone_controller)/launch/safe_zone_controller.launch" />
<include file="$(find pressure_sensor)/launch/pressure_sensor_mock.launch" />
<node name="Cobot1_Controller" pkg="cobot_1" type="CobotController" respawn="false" output="screen" required="true" >
<rosparam command="load" file="$(find cobot_1)/config/cobot1_config.yaml" />
</node>
</launch>
launch/cobot1_mock.launch 0 → 100644
+ 22
− 0
View file @ a87d8246
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<include file="$(find safe_zone_controller)/launch/safe_zone_controller.launch" />
<include file="$(find pressure_sensor)/launch/pressure_sensor_mock.launch" />
<node name="Cobot1_Controller" pkg="cobot_1" type="CobotController" respawn="false" output="screen">
<rosparam command="load" file="$(find cobot_1)/config/cobot1_config.yaml" />
</node>
<!-- Enforce joint space planning. Otherwise an orientation constraint would always result in cartesian planning -->
<param name="/move_group/panda_arm/enforce_joint_model_state_space" value="true"/>
<!-- The execution_duration_monitoring sometimes aborts a valid trajectory execution -->
<param name="move_group/trajectory_execution/execution_duration_monitoring" value="false" />
</launch>
launch/cobot1_mock_test.launch 0 → 100644
+ 22
− 0
View file @ a87d8246
<launch>
<include file="$(find panda_simulation)/launch/simulation.launch"/>
<include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
<include file="$(find safe_zone_controller)/launch/safe_zone_controller.launch" />
<include file="$(find pressure_sensor)/launch/pressure_sensor_mock.launch" />
<node name="Cobot1_TRONAdapter" pkg="cobot_1" type="TRON_Adapter" respawn="false" output="screen"/>
<node name="Cobot1_Controller" pkg="cobot_1" type="CobotController" respawn="false" output="screen">
<rosparam command="load" file="$(find cobot_1)/config/cobot1_config.yaml" />
</node>
<!-- Enforce joint space planning. Otherwise an orientation constraint would always result in cartesian planning -->
<param name="/move_group/panda_arm/enforce_joint_model_state_space" value="true"/>
<!-- The execution_duration_monitoring sometimes aborts a valid trajectory execution -->
<param name="move_group/trajectory_execution/execution_duration_monitoring" value="false" />
</launch>
models/box.urdf 0 → 100644
+ 30
− 0
View file @ a87d8246
<robot name="box">
<link name="world"/>
<gazebo reference="world">
<static>true</static>
</gazebo>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="real_box"/>
</joint>
<link name="real_box">
<inertial>
<origin xyz="0.45 -0.2 0.0" />
<mass value="1.0" />
<inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
</inertial>
<visual>
<origin xyz="0.45 -0.2 0.0"/>
<geometry>
<box size="0.4 0.1 0.3" />
</geometry>
</visual>
<collision>
<origin xyz="0.45 -0.2 0.0"/>
<geometry>
<box size="0.4 0.1 0.3" />
</geometry>
</collision>
</link>
</robot>
\ No newline at end of file
package.xml 0 → 100644
+ 57
− 0
View file @ a87d8246
<?xml version="1.0"?>
<package format="2">
<name>cobot_1</name>
<version>0.0.0</version>
<description>The cobot_1 package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="jim.molkenthin@mailbox.tu-dresden.de">Jim Molkenthin</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>controller_manager</depend>
<depend>effort_controllers</depend>
<depend>gazebo_ros</depend>
<depend>gazebo_ros_control</depend>
<depend>joint_state_controller</depend>
<depend>joint_state_publisher</depend>
<depend>joint_trajectory_controller</depend>
<depend>robot_state_publisher</depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<depend>tf</depend>
<depend>xacro</depend>
<depend>franka_gripper</depend>
<depend>moveit_simple_controller_manager</depend>
<!-- CUSTOM -->
<depend>moveit_core</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_ros_perception</depend>
<depend>moveit_visual_tools</depend>
<depend>controller_interface</depend>
<depend>hardware_interface</depend>
<depend>simulation_util</depend>
<depend>planning_util</depend>
<depend>cooperation_msgs</depend>
<exec_depend>pluginlib</exec_depend>
<depend>pick_place</depend>
<depend>constraints</depend>
<depend>object_creator</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
rosdoc.yaml 0 → 100644
+ 4
− 0
View file @ a87d8246
- builder: doxygen
name: C++ API
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
homepage: https://st.inf.tu-dresden.de/ceti-robots
scripts/loop.sh 0 → 100644
+ 6
− 0
View file @ a87d8246
#!/bin/bash
for i in {1..25}
do
echo "LAUNCH FILE LOOP $i"
roslaunch cobot_1 cobot1_minimal_mock.launch > log-run-$i.txt 2>&1
done
src/Cobot.cpp 0 → 100644
+ 312
− 0
View file @ a87d8246
//
// Created by jim on 04.01.21.
//
#include "Cobot.h"
#include "PickPlace.h"
#include <TrajectoryTool.h>
#include <grasp_util.h>
Cobot::Cobot(moveit::planning_interface::MoveGroupInterface &group){
this->start_state_joint_values = group.getCurrentJointValues();
this->orientationConstraint = OrientationConstraint();
this->proximityConstraint = ProximityConstraint();
this->velocityConstraint = VelocityConstraint();
this->accelerationConstraint = AccelerationConstraint();
}
bool Cobot::moveToStart(moveit::planning_interface::MoveGroupInterface &group, bool with_constraints) {
if(!with_constraints) {
this->orientationConstraint.remove(group);
this->velocityConstraint.remove(group);
this->accelerationConstraint.remove(group);
}
group.setJointValueTarget(this->start_state_joint_values);
group.plan(this->movePlan);
return group.execute(this->movePlan) == moveit_msgs::MoveItErrorCodes::SUCCESS;
}
bool Cobot::pickBottle(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject bottle, int retry_on_failure_count, double retry_planning_time_factor) {
ROS_INFO("\n-----------------------\nPicking up the bottle...\n-----------------------");
int tries = 0;
bool success = false;
// GraspUtil grasp_util;
while(!success && tries < retry_on_failure_count){
/* double open_amount = bottle.primitives[0].dimensions[0] < bottle.primitives[0].dimensions[1] ? bottle.primitives[0].dimensions[0] : bottle.primitives[0].dimensions[1];
geometry_msgs::Vector3 dimensions;
dimensions.x = bottle.primitives[0].dimensions[0];
dimensions.y = bottle.primitives[0].dimensions[1];
dimensions.z = bottle.primitives[0].dimensions[2];
success = grasp_util.pickObjectOnRobotfrontFromSide(group, bottle.id, bottle.pose, open_amount, dimensions, 1);*/
success = PickPlace::pick_x(group, bottle) == moveit_msgs::MoveItErrorCodes::SUCCESS;
if(!success) {
ROS_ERROR("[CC][Cobot1] Retry bottle pick");
}
tries++;
}
// hold bottle upright
this->orientationConstraint.apply(group);
// lower acceleration to prevent spilling
this->accelerationConstraint.apply(group);
return success;
}
bool Cobot::fillGlass(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject glass, int retry_on_failure_count, double retry_planning_time_factor) {
ROS_INFO("\n-----------------------\nFilling the glass...\n-----------------------");
moveit::planning_interface::MoveGroupInterface::Plan plan;
geometry_msgs::Pose start_pose;
group.setPlanningTime(group.getPlanningTime() * retry_planning_time_factor);
int y_direction = 1;
double planning_time;
double eef_rotation = INFINITY;
double pouring_rotation_value = (M_PI * 5/8);
while(ros::ok()) {
// Move to pouring pose
start_pose.position = glass.primitive_poses[0].position;
start_pose.position.z = start_pose.position.z + glass.primitives[0].dimensions[2]/2 + 0.09; // TODO refactor magic numbers
start_pose.position.y = start_pose.position.y + (glass.primitives[0].dimensions[1]/2 + 0.04) * y_direction; // hold next to the glass
start_pose.position.x = start_pose.position.x - 0.1;
start_pose.orientation = group.getCurrentPose().pose.orientation;
group.setPoseTarget(start_pose);
// If the planning fails retry
if(group.plan(plan) == moveit_msgs::MoveItErrorCodes::SUCCESS) {
// Get the end effector rotation in the goal state
eef_rotation = plan.trajectory_.joint_trajectory.points.back().positions[6];
if(std::abs(eef_rotation + pouring_rotation_value * y_direction) < 2.89725){
// 2.89725 is the maximal rotation for panda_joint7 -> The filling rotation should be executable
ROS_INFO("\n-----------------------\nMove to filling position...\n-----------------------");
group.move();
group.setPlanningTime(25);
break;
}
}
ROS_ERROR("[CC][Cobot1] Failed placing the bottle next to the glass");
// Retry from the opposite side of the glass
y_direction *= -1;
planning_time += 5;
group.setPlanningTime(planning_time);
}
this->orientationConstraint.remove(group); // Remove orientation constraints to allow rotation on eef joint
this->velocityConstraint.apply(group); // Constrain the velocity to slowly pour the liquid
// Get the current joint values and add a rotation to the end effector
std::vector<double> joint_values;
group.getCurrentState()->copyJointGroupPositions(group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName()), joint_values);
double preFillJointValue = joint_values[6];
joint_values[6] += pouring_rotation_value * y_direction ;
group.setJointValueTarget(joint_values);
// If planning fails retry
while(group.plan(plan) != moveit_msgs::MoveItErrorCodes::SUCCESS) {
ROS_WARN("[CC][Cobot1] Failed placing the bottle! Retrying...");
}
ROS_INFO("\n-----------------------\nFill the glass...\n-----------------------");
group.move();
// Hold a second to finish pouring
ros::Duration(1.0).sleep();
// rotate back
joint_values[6] = preFillJointValue;
group.setJointValueTarget(joint_values);
int tries = 0;
bool success = false;
while(!success && tries < retry_on_failure_count){
success = group.plan(plan) == moveit_msgs::MoveItErrorCodes::SUCCESS;
if(!success){
ROS_WARN("[CC][Cobot1] Retry to stop filling...");
}
tries++;
}
if(tries == retry_on_failure_count){
ROS_ERROR("[CC][Cobot1] Glass fill planning failed.");
return false;
}
ROS_INFO("\n-----------------------\nStop filling...\n-----------------------");
group.move();
this->accelerationConstraint.remove(group); // Since the bottle is empty now, the acceleration constraint is no longer necessary
this->velocityConstraint.remove(group); // Remove the velocity constraint
this->orientationConstraint.apply(group); // Reapply orientation to keep the bottle upright again
group.setPlanningTime(group.getPlanningTime() / retry_planning_time_factor);
return true;
}
bool Cobot::placeBottle(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject bottle, int retry_on_failure_count, double retry_planning_time_factor) {
ROS_INFO("\n-----------------------\nPlace the bottle...\n-----------------------");
// Get the bottles place position
ros::NodeHandle nh;
nh.getParam("/object/bottle/place/x", bottle.pose.position.x);
nh.getParam("/object/bottle/place/y", bottle.pose.position.y);
nh.getParam("/object/bottle/place/z", bottle.pose.position.z);
group.setGoalPositionTolerance(0.05);
group.setPlanningTime(group.getPlanningTime() * retry_planning_time_factor);
int tries = 0;
bool success = false;
while(!success && tries < retry_on_failure_count){
// Place the bottle on its original position
success = PickPlace::place(group, bottle, bottle.pose.position) == moveit_msgs::MoveItErrorCodes::SUCCESS;
if(!success){
ROS_ERROR("[CC][Cobot1] Retry bottle ṕlace");
}
tries++;
}
this->orientationConstraint.remove(group); // After placing the bottle constraining the orientation is unnecessary
group.setGoalPositionTolerance(0.01);
group.setPlanningTime(group.getPlanningTime() / retry_planning_time_factor);
return success;
}
bool Cobot::pickGlass(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject glass, int retry_on_failure_count, double retry_planning_time_factor) {
ROS_INFO("\n-----------------------\nPick up the glass...\n-----------------------");
group.setPlanningTime(group.getPlanningTime() * retry_planning_time_factor);
// Try to pick up the glass
int tries = 0;
bool success = false;
while(!success && tries < retry_on_failure_count){
// Place the bottle on its original position
success = PickPlace::pick_z(group, glass) == moveit_msgs::MoveItErrorCodes::SUCCESS;
if(!success){
ROS_ERROR("[CC][Cobot1] Retry bottle pick");
}
tries++;
}
this->orientationConstraint.apply(group); // Hold the glass upright
this->accelerationConstraint.apply(group); // Constrain acceleration to prevent spilling
this->proximityConstraint.set_min_distance(0.1);
this->proximityConstraint.apply(group); // Keep a minimum distance of 10cm to the table
group.setPlanningTime(group.getPlanningTime() / retry_planning_time_factor);
return success;
}
bool Cobot::placeGlass(moveit::planning_interface::MoveGroupInterface &group, moveit_msgs::CollisionObject glass,
geometry_msgs::Point target_place_position, int retry_on_failure_count, double retry_planning_time_factor) {
ROS_INFO("\n-----------------------\nPlace the glass...\n-----------------------");
this->proximityConstraint.remove(group); // Remove proximity constraint to allow the placement on the table
this->orientationConstraint.apply(group); // Reapply orientation because removing the proximity constraint removes all path constraints
this->accelerationConstraint.set_scaling_factor(0.05);
this->accelerationConstraint.apply(group);
geometry_msgs::Pose pre_place_pose;
moveit::planning_interface::MoveGroupInterface::Plan pre_place_plan;
group.setPlanningTime(group.getPlanningTime() * retry_planning_time_factor);
// Move near the placing position
pre_place_pose.position = target_place_position;
// x: 0.106 y: 0 z: 0.3
pre_place_pose.position.z = pre_place_pose.position.z + 0.3;
pre_place_pose.orientation = group.getCurrentPose().pose.orientation;
group.setPoseTarget(pre_place_pose);
group.setGoalPositionTolerance(0.1);
int pre_place_retries = 0;
bool pre_place_success = false;
while(!pre_place_success && pre_place_retries < retry_on_failure_count){
pre_place_success = group.plan(pre_place_plan) == moveit_msgs::MoveItErrorCodes::SUCCESS;
ROS_ERROR("[CC][Cobot1] Retry planning for pre place position...");
pre_place_retries++;
}
if(pre_place_retries == retry_on_failure_count){
ROS_ERROR("[CC][Cobot1] Glass pre-placement planning failed.");
return false;
}
group.execute(pre_place_plan);
group.setGoalPositionTolerance(0.02);
ROS_ERROR("[CC][Cobot1] Place the glass...");
int tries = 0;
bool placeSuccess = false;
while(tries < retry_on_failure_count && !placeSuccess){
placeSuccess = (PickPlace::place(group, glass, target_place_position) == moveit_msgs::MoveItErrorCodes::SUCCESS);
if(placeSuccess){
ROS_INFO("[CC][Cobot1] Successfull planning for glass placement");
break;
}else{
ROS_WARN("[CC][Cobot1] Retry planning for glass placement");
}
if(!placeSuccess && tries == 0){
ROS_WARN("[CC][Cobot1] Stepping back.");
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
auto currentPose = group.getCurrentPose();
currentPose.pose.position.z = currentPose.pose.position.z + 0.1;
group.setPoseTarget(currentPose);
group.plan(my_plan);
group.move();
}
tries++;
}
this->orientationConstraint.remove(group);
this->accelerationConstraint.remove(group);
group.setPlanningTime(group.getPlanningTime() / retry_planning_time_factor);
if(tries == retry_on_failure_count){
ROS_ERROR("[CC][Cobot1] Reverting to last state.");
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
auto currentPose = group.getCurrentPose();
currentPose.pose.position.z = currentPose.pose.position.z - 0.1;
group.setPoseTarget(currentPose);
group.execute(my_plan);
moveit::planning_interface::MoveGroupInterface::Plan reverted_pre_place_plan;
group.execute(TrajectoryTool::invert_plan(pre_place_plan, group, "panda_arm"));
ROS_ERROR("[CC][Cobot1] Glass placement planning failed.");
return false;
}
return true;
}
src/CobotController.cpp 0 → 100644
+ 224
− 0
View file @ a87d8246
//
// Created by jim on 04.01.21.
//
// ROS
#include <ros/ros.h>
// MoveIt
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/CollisionObject.h>
// Visual tools
#include <moveit_visual_tools/moveit_visual_tools.h>
// Cooperation messages
#include <cooperation_msgs/PressureMsg.h>
#include <cooperation_msgs/SafeZone.h>
#include "Cobot.h"
#include "ObjectCreator.h"
bool pressed = false;
int RETRY_ON_FAILURE_COUNT = 5;
double RETRY_PLANNING_TIME_FACTOR = 1;
/**
* Callback function for incoming messages on the "pressure1" topic
* @param msg the received message
*/
void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) {
ROS_INFO_NAMED("Pressure1", "Received change in pressure 1 state");
pressed = msg->pressed;
}
/**
* Call the safe_zone_service and try to occupy the safe zone
* @param safeZoneClient client that can call the safe_zone_service
* @return whether the occupation was successful
*/
bool occupySafeZone(ros::ServiceClient &safeZoneClient) {
cooperation_msgs::SafeZone srv;
srv.request.occupy = true;
if (safeZoneClient.call(srv)) {
ROS_INFO_NAMED("Safe zone service response", "Allowed to enter the safe zone: %d", srv.response.success);
return srv.response.success;
} else {
ROS_ERROR("Failed to call safe_zone_controller");
return false;
}
}
/**
* Call the safe_zone_service and try to unblock the safe zone
* @param safeZoneClient client that can call the safe_zone_service
* @return whether the unblock was successful
*/
bool freeSafeZone(ros::ServiceClient &safeZoneClient) {
cooperation_msgs::SafeZone srv;
srv.request.occupy = false;
if (safeZoneClient.call(srv)) {
ROS_INFO_NAMED("Safe zone service response", "Unblocked the safe zone: %d", srv.response.success);
return srv.response.success;
} else {
ROS_ERROR("Failed to call safe_zone_controller");
return false;
}
}
int main(int argc, char **argv) {
// ROS initialization
ros::init(argc, argv, "cobot1_controller");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(3);
spinner.start();
ros::Duration(3.0).sleep();
moveit::planning_interface::MoveGroupInterface group("panda_arm");
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
group.setPlanningTime(25.0);
group.setNumPlanningAttempts(3);
group.setGoalTolerance(0.01);
// group.setPlannerId("RRTstarkConfigDefault");
// group.setPlanningPipelineId()
// Setting up retries
node_handle.getParam("/planning/retries", RETRY_ON_FAILURE_COUNT);
node_handle.getParam("/planning/time_factor", RETRY_PLANNING_TIME_FACTOR);
ROS_INFO_STREAM("[CC] Setting planning retries to " << RETRY_ON_FAILURE_COUNT);
ROS_INFO_STREAM("[CC] Setting planning time factor to " << RETRY_PLANNING_TIME_FACTOR);
// Subscribe to pressure sensor 1
ros::Subscriber pressure_1_sub = node_handle.subscribe("pressure", 10, on_pressure);
// Create the safe zone client to occupy the safe zone if needed
ros::ServiceClient safeZoneClient = node_handle.serviceClient<cooperation_msgs::SafeZone>("safe_zone_controller");
// Add objects to the environment
ObjectCreator oc(planning_scene_interface);
oc.createTable(ObjectConfig("/object/table/"), "table1");
moveit_msgs::CollisionObject glass = oc.createBox(ObjectConfig("/object/glass/"), "glass", true);
geometry_msgs::Point original_pick_location;
original_pick_location.x = glass.primitive_poses[0].position.x;
original_pick_location.y = glass.primitive_poses[0].position.y;
original_pick_location.z = glass.primitive_poses[0].position.z;
moveit_msgs::CollisionObject bottle = oc.createBox(ObjectConfig("/object/bottle/"), "bottle", true);
moveit_msgs::CollisionObject pressure_sensor_1 = oc.createBox(ObjectConfig("/object/pressure_sensor1/"),
"pressure_sensor1", true);
moveit_msgs::CollisionObject pressure_sensor_2 = oc.createBox(ObjectConfig("/object/pressure_sensor2/"),
"pressure_sensor2", true);
SafeZoneConstraint safeZone(planning_scene_interface, ObjectConfig("/object/safezone/"));
safeZone.apply(group);
// Attach the table to the robot to allow collisions between the base link and the table
std::vector<std::string> touch_links;
touch_links.push_back("panda_link1");
touch_links.push_back("panda_link0");
group.attachObject("table1", "panda_link0", touch_links);
// Set support surface as table1.
group.setSupportSurfaceName("table1");
ros::Duration(2.0).sleep();
// Initialize the cobot
Cobot cobot(group);
while (ros::ok()) {
cobot.moveToStart(group, false);
while (!pressed) {
// wait while no glass present
ros::spinOnce();
ros::Duration(0.5).sleep();
}
if (!cobot.pickBottle(group, bottle, RETRY_ON_FAILURE_COUNT, RETRY_PLANNING_TIME_FACTOR)) {
ROS_ERROR("[CC] Failed picking up the bottle.");
return 0;
}
if (!cobot.fillGlass(group, glass, RETRY_ON_FAILURE_COUNT, RETRY_PLANNING_TIME_FACTOR)) {
ROS_ERROR("[CC] Failed filling the glass..Retrying");
return 0;
}
if (!cobot.placeBottle(group, bottle, RETRY_ON_FAILURE_COUNT, RETRY_PLANNING_TIME_FACTOR)) {
ROS_ERROR("[CC] Failed placing the bottle..Retrying");
return 0;
}
ros::Duration(0.5).sleep();
if (!cobot.pickGlass(group, glass, RETRY_ON_FAILURE_COUNT, RETRY_PLANNING_TIME_FACTOR)) {
ROS_ERROR("[CC] FATAL ... Failed to pick glass.");
return 0;
}
while (!occupySafeZone(safeZoneClient)) { // if not allowed to enter the safe zone, wait and try again
ros::Duration(0.5).sleep();
}
safeZone.remove(group); // remove the safe zone obstacle if allowed to enter
ros::Duration(0.5).sleep();
// Glass will be place on the opposite site of the robot
geometry_msgs::Point target_place_position;
node_handle.getParam("/object/glass/place/x", target_place_position.x);
node_handle.getParam("/object/glass/place/y", target_place_position.y);
node_handle.getParam("/object/glass/place/z", target_place_position.z);
int place_glass_retry = 0;
while (!cobot.placeGlass(group, glass, target_place_position, RETRY_ON_FAILURE_COUNT, RETRY_PLANNING_TIME_FACTOR)) {
ROS_ERROR("[CC] Falling back ... cannot place.");
// do fallback
if (!cobot.placeGlass(group, glass, original_pick_location, RETRY_ON_FAILURE_COUNT, RETRY_PLANNING_TIME_FACTOR)) {
ROS_ERROR("[CC] FATAL ... Failed to do fallback.");
return 0;
} else {
ROS_ERROR("[CC] Successfully executed fallback.");
ROS_ERROR("[CC] Re-Picking the glass");
if (!cobot.pickGlass(group, glass, RETRY_ON_FAILURE_COUNT, RETRY_PLANNING_TIME_FACTOR)) {
ROS_ERROR("[CC] FATAL cannot repick");
return 0;
}
ROS_INFO("[CC] Re-Pick successfull.");
}
ROS_ERROR("[CC] Failed placing the glass ... Retrying");
place_glass_retry++;
if (place_glass_retry == RETRY_ON_FAILURE_COUNT) {
ROS_ERROR("[CC] FATAL ... Failed to place glass.");
return 0;
}
}
cobot.moveToStart(group, false);
safeZone.apply(group); // reapply the safe zone constraint
freeSafeZone(safeZoneClient); // Unblock the safe zone for other robots
ROS_INFO_NAMED("FINISHED", "Completed demo execution");
ROS_ERROR("[CC] FINISHED");
ros::shutdown();
}
return 0;
}
\ No newline at end of file
src/tron_adapter.cpp 0 → 100644
+ 504
− 0
View file @ a87d8246
#include <ros/ros.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <std_msgs/Int32.h>
#include <boost/make_shared.hpp>
#include <memory>
#include <cooperation_msgs/PressureMsg.h>
const std::string IP = "127.0.0.1";
const uint16_t PORT = 8080;
typedef uint8_t byte;
// some helper functions -----------------------------------------------
// log bytes sent/received
inline void byte_info(const byte* buf, int buf_length, bool send=true){
std::stringstream strstr;
strstr << (send ? "sending" : "received") << " bytes:";
for (int i = 0; i < buf_length; i++) strstr << " " << (int)buf[i];
ROS_INFO(strstr.str().c_str());
}
// gets count bytes from socket file descriptor (with timeout)
const double SECONDS_BEFORE_TIMEOUT = 30;
std::unique_ptr<byte[]> get_bytes_socket(int fd, int count){
std::unique_ptr<byte[]> arr = std::make_unique<byte[]>(count);
int already_read = 0;
ros::Time start_time = ros::Time::now();
int success = 0;
while (already_read < count) {
success = recv(fd, &arr[already_read], count-already_read, MSG_DONTWAIT);
if (success == 0) throw "connection was closed by TRON";
if (success == -1){
if (ros::Time::now().toSec() - start_time.toSec() >= SECONDS_BEFORE_TIMEOUT)
throw "timeout while reading bytes from socket file descriptor";
continue;
}
already_read += success; // read returns number of bytes read
}
byte_info(arr.get(), already_read, false);
return arr; // no explicit move needed since return value is rvalue
};
inline int32_t network_bytes_to_int_32(byte *buf){
uint32_t h = ntohl(*reinterpret_cast<uint32_t*>(buf));
return *reinterpret_cast<int32_t*>(&h);
}
inline uint16_t network_bytes_to_uint_16(byte *buf) {
return ntohs(*reinterpret_cast<uint16_t*>(buf));
}
// wrapping get_bytes_socket for converting to 32 bit integer
int32_t get_int_socket(int fd) {
auto ack = get_bytes_socket(fd, 4);
return network_bytes_to_int_32(ack.get());
}
// converts num to network order and adds it to byte array starting from index
void add_int32_in_network_order(int32_t num, byte *buf, int index){
uint32_t n = htonl(*reinterpret_cast<uint32_t*>(&num));
byte* bytes = reinterpret_cast<byte*>(&n);
buf[index] = bytes[0];
buf[++index] = bytes[1];
buf[++index] = bytes[2];
buf[++index] = bytes[3];
}
// wraps write() for printing and throwing on errors
inline void send_bytes(int fd, const byte *buf, int length){
byte_info(buf, length);
int ret = write(fd, (void*) buf, length);
if (ret < 0) throw "writing failed";
}
// returns false if nothing more to read and true if 4 bytes are read successfully
// used to reduce overhead in testing phase
inline bool read_4_bytes_nonblock(int fd, byte *buf) {
int bytes_recv = recv(fd, buf, 4, MSG_DONTWAIT);
if (bytes_recv == -1) return false; // nothing more to read
if (bytes_recv == 0) throw "connection was closed";
if (bytes_recv != 4) throw "could not read full 4 bytes";
byte_info(buf, 4, false);
return true;
}
// returns file descriptor
int create_connected_socket(std::string IP, uint16_t port){
int socketfd;
if ((socketfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
throw "failed to create socket";
}
ROS_INFO("socket created successfully");
struct sockaddr_in addr;
addr.sin_family = AF_INET;
addr.sin_port = htons(port);
{
int x = inet_pton(AF_INET, IP.c_str(), &addr.sin_addr);
if (x != 1) {
throw "IP could not be converted";
}
}
if (connect(socketfd, (struct sockaddr*)&addr, sizeof(sockaddr_in)) < 0) {
throw "failed to connect";
}
ROS_INFO("successfully connected");
return socketfd;
}
// consts for TRON ----------------------------------------------------------
const byte GET_ERROR_MSG = 127;
const byte DECL_CHAN_INPUT = 1;
const byte DECL_CHAN_OUTPUT = 2;
const byte ADD_VAR_TO_INPUT = 3;
const byte ADD_VAR_TO_OUTPUT = 4;
const byte SET_TIME_UNIT = 5;
const byte SET_TIMEOUT = 6;
const byte REQUEST_START = 64;
const byte ANSWER_START = 0;
const int32_t ACK_SINGLE = 1 << 31; // 32 Bit int with most significant bit set to 1
// global vars -------------------------------------------------------------
// ROS uses little endian for its messages
const bool SYS_IS_BIG_ENDIAN = htonl(47) == 47;
int socket_fd;
struct Channel {
std::string name;
int32_t identifier;
bool is_input;
std::vector<std::string> vars; // associated variables in Uppaal
Channel(std::string name, int32_t id, bool is_input) : name(name), identifier(id), is_input(is_input){};
};
// used in struct Mapping for convenience
std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name);
void add_var_to_channel(Channel& chan, bool is_input, std::string var);
struct Mapping {
std::string topic; // ROS topic name
// offset in byte for next value (from last value)
std::vector<int> byte_offset;
// custom conversion function used if data is not int32 and therefore
// need to be converted to an int32 first
// nullptr if not used
// needs to make sure count of bytes used gets added to *int (parameter)
// to indicate beginning of next field
std::vector<int32_t (*)(byte*, int*)> converters_to_TRON;
// similarly the conversion to a topic field takes the value from TRON,
// a pointer to the buffer at which to add the bytes and
// a pointer to an integer to increment by the amount of bytes added
std::vector<void (*)(int32_t, byte*, int*)> converters_to_topics;
Channel channel = Channel("", 0, true); // TRON channel
/* note: all vectors need to have the same size
example:
topic = "example_topic"
byte_offset = [4, 2]
converters = *some pointer converting 8 bytes to int32, nullptr
channel = struct Channel "example channel"
This example maps any message from topic "example_topic" to "example_channel".
8 Bytes starting from index 4 are converted via the given pointer
and mapped to the first variable of the corresponding channel.
The conversion function also adds 8 to the current position,
thus the next field starts 8 bytes + 2 bytes for offset after the last
known position, which was 4.
Because the second converter points to nullptr, it is considered an int32,
which is mapped to the second variable of the channel.
The next position is incremented by 4 if there is no conversion function given.
Note that the first 4 Bytes of the topics message are ignored in this example.
*/
// Callback used if TRON sends some message
void(*input_callback)(Mapping& map, int32_t*) = nullptr;
Mapping(std::string topic, std::string channel, bool channelIsInput){
this->topic = topic;
this->channel = *send_channel_decl_msg(channelIsInput, channel).get();
}
void add_var_to_mapping(std::string name_tron, int byte_offset,
int32_t (*conv_to_TRON)(byte*, int*) = nullptr,
void (*conv_to_topic)(int32_t, byte*, int*) = nullptr){
add_var_to_channel(this->channel, this->channel.is_input, name_tron);
this->byte_offset.push_back(byte_offset);
this->converters_to_TRON.push_back(conv_to_TRON);
this->converters_to_topics.push_back(conv_to_topic);
}
};
std::list<Mapping> mappings;
std::vector<ros::Publisher> input_publishers;
template <class T>
void publish_to_topic(std::string topic, boost::shared_ptr<T> ptr) {
for (ros::Publisher& pub : input_publishers)
if (pub.getTopic() == topic) {
pub.publish(ptr);
return;
}
throw "did not find publisher for topic";
}
// callback publishing to topics like defined in mapping, used for input_callback
template<class T>
void mapping_callback_to_topic(Mapping& map, int32_t* vars){
boost::shared_ptr<T> shared_ptr = boost::make_shared<T>();
byte* ptr = reinterpret_cast<byte*>(shared_ptr.get());
int next_pos = 0;
for (int i = 0; i < map.channel.vars.size(); i++){
int32_t val = vars[i];
next_pos += map.byte_offset[i];
if (map.converters_to_topics[i] == nullptr) {
byte* val_bytes;
if (SYS_IS_BIG_ENDIAN){
uint32_t switched_byte_order = htonl(*reinterpret_cast<uint32_t*>(&val));
val = *reinterpret_cast<int32_t*>(&switched_byte_order);
}
val_bytes = reinterpret_cast<byte*>(&val);
ptr[next_pos++] = val_bytes[0];
ptr[next_pos++] = val_bytes[1];
ptr[next_pos++] = val_bytes[2];
ptr[next_pos++] = val_bytes[3];
} else {
map.converters_to_topics[i](val, &ptr[next_pos], &next_pos);
}
}
publish_to_topic<T>(map.topic, shared_ptr);
}
// subscribers already have callbacks
std::vector<ros::Subscriber> output_subscribers;
// keep track of acknowledgements that are missing
/* note: since communication is asynchronous this value can only be
compared reliably with 0 after testing is terminated */
/* note: TRON only acknowledges if virtual clock is used, which it is not (yet) */
int acks_missing = 0;
void get_error_msg(int32_t errorcode) {
ROS_WARN("got error, trying to get corresponding message");
byte get_err_msg_msg[5];
get_err_msg_msg[0] = GET_ERROR_MSG; // get error msg code
add_int32_in_network_order(errorcode, get_err_msg_msg, 1);
send_bytes(socket_fd, get_err_msg_msg, 5); // connection is closed after this write?
byte err_msg_length = get_bytes_socket(socket_fd, 1)[0];
auto err_msg = get_bytes_socket(socket_fd, err_msg_length);
std::string msg_str = std::string(reinterpret_cast<char*>(err_msg.get()), (size_t) err_msg_length);
ROS_FATAL("TRON sent error message: %s", msg_str.c_str());
throw "got error from TRON";
}
void add_var_to_channel(Channel& chan, bool is_input, std::string var) {
byte msg[6 + var.length()];
msg[0] = is_input ? ADD_VAR_TO_INPUT : ADD_VAR_TO_OUTPUT;
add_int32_in_network_order(chan.identifier, msg, 1);
msg[5] = (byte) var.length();
for (int i = 0; i < var.length(); i++) msg[6+i] = var.at(i);
ROS_INFO("attaching variable %s to channel %s", var.c_str(), chan.name.c_str());
send_bytes(socket_fd, msg, 6 + var.length());
int32_t ack = get_int_socket(socket_fd);
if (ack < 0) get_error_msg(ack);
ROS_INFO("success: attached variable");
chan.vars.push_back(var);
}
std::unique_ptr<Channel> send_channel_decl_msg(bool is_input, std::string name) {
// prepare packet
size_t msg_length = 2 + name.length();
byte msg[msg_length];
msg[0] = is_input ? DECL_CHAN_INPUT : DECL_CHAN_OUTPUT;
msg[1] = name.length();
for (int i = 2, c = 0; i < msg_length; i++, c++) msg[i] = name[c];
// send packet
ROS_INFO("declaring channel %s as %s", name.c_str(), (is_input ? "input" : "output"));
byte_info(msg, msg_length);
write(socket_fd, (void*) msg, msg_length);
// get answer from TRON
int32_t channel_identifier = get_int_socket(socket_fd);
if (channel_identifier < 0) { // error handling
get_error_msg(channel_identifier);
}
if (channel_identifier == 0) throw "did not get channel identifier";
// assigned channel ID successfully
ROS_INFO("success: identifier for channel %s is %i", name.c_str(), channel_identifier);
return std::make_unique<Channel>(name, channel_identifier, is_input);
}
void set_time_unit_and_timeout(uint64_t microseconds, int32_t timeout){
byte msg[9];
msg[0] = SET_TIME_UNIT;
byte *microseconds_bytes = reinterpret_cast<byte*>(&microseconds);
// htonl does not exist for long int
if (SYS_IS_BIG_ENDIAN) {
for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[i];
} else {
for (int i = 0; i < 8; i++) msg[i+1] = microseconds_bytes[7-i];
}
ROS_INFO("setting time unit: %li microseconds", microseconds);
send_bytes(socket_fd, msg, 9);
int32_t ack = get_int_socket(socket_fd);
if (ack != 0) get_error_msg(ack);
ROS_INFO("success: set time unit");
msg[0] = SET_TIMEOUT;
add_int32_in_network_order(timeout, msg, 1);
ROS_INFO("setting timeout to %i units", timeout);
send_bytes(socket_fd, msg, 5);
ack = get_int_socket(socket_fd);
if (ack != 0) get_error_msg(ack);
ROS_INFO("success: set timeout");
}
void request_start() {
/* documentation confuses codes for start and getErrorMessage, actually used:
64 is start
127 is gerErrorMessage */
ROS_INFO("requesting start");
byte start = REQUEST_START;
send_bytes(socket_fd, &start, 1);
byte answer = get_bytes_socket(socket_fd, 1)[0];
if (answer != ANSWER_START) throw "starting failed";
ROS_INFO("success: starting test phase");
}
void report_now(Channel& chan, uint16_t var_count = 0, int32_t *vars = nullptr){
std::unique_ptr<byte[]> msg = std::make_unique<byte[]>(6 + 4 * var_count);
add_int32_in_network_order(chan.identifier, msg.get(), 0);
unsigned short var_count_network_order = htons(var_count);
byte *var_count_bytes = reinterpret_cast<byte*>(&var_count_network_order);
msg[4] = var_count_bytes[0];
msg[5] = var_count_bytes[1];
for (unsigned short i = 0; i < var_count; i++)
add_int32_in_network_order(vars[i], msg.get(), 6 + i * 4);
ROS_INFO("sending to output channel %s", chan.name.c_str());
if (var_count == 0) ROS_INFO("no variables attached");
for (unsigned short i = 0; i < var_count; i++)
ROS_INFO("attached value %i to variable %s", vars[i], chan.vars[i].c_str());
send_bytes(socket_fd, msg.get(), 6 + 4 * var_count);
acks_missing++;
}
// callback reports to TRON like defined in mappings
template<class T>
void mappings_callback_to_TRON(const ros::MessageEvent<T>& event){
std::string topic = event.getConnectionHeader().at("topic");
byte *bytes = reinterpret_cast<byte*>(event.getMessage().get());
for (Mapping& mapping : mappings) {
if (mapping.topic.c_str() == topic && !mapping.channel.is_input) {
int var_count = mapping.byte_offset.size();
int32_t vars[var_count * 4];
int next_pos = 0;
for (int i = 0; i < var_count; i++) {
next_pos += mapping.byte_offset[i];
if (mapping.converters_to_TRON[i] == nullptr) {
if (SYS_IS_BIG_ENDIAN)
vars[i] = network_bytes_to_int_32(&bytes[next_pos]);
else vars[i] = *reinterpret_cast<int32_t*>(&bytes[next_pos]);
next_pos += 4;
continue;
} else {
vars[i] = mapping.converters_to_TRON[i](&bytes[next_pos], &next_pos);
}
}
report_now(mapping.channel, var_count, vars);
}
}
}
void on_pressure(const cooperation_msgs::PressureMsg::ConstPtr &msg) {
if (msg->pressed)
for (Mapping& map : mappings) {
if (map.channel.name == "pressed" && map.channel.is_input == false)
report_now(map.channel);
}
}
void configuration_phase(ros::NodeHandle& nh){
/* note: for configuration phase maximum message length is 256 Bytes,
therefore heap allocation can be avoided most of the time in called functions */
// note: since we are not an actual client (or server)
// we need to use the high level packet::*Action* messages.
// custom callbacks are implemented in order to not worry about header size
Mapping map = Mapping("/pressure", "pressed", false);
mappings.push_back(map);
output_subscribers.push_back(nh.subscribe("/pressure", 10, on_pressure));
// not obvious in documentation: local variables are not supported
// add_var_to_channel(socketfd, "ausloesen", "lokal");
uint64_t microseconds = 1000000; // one second
// documentation states 2 signed integers are used for some reason
set_time_unit_and_timeout(microseconds, 300);
// wait till subscribers initialized
for (ros::Publisher& pub : input_publishers) {
while (pub.getNumSubscribers() == 0) { ros::Duration(1).sleep(); };
}
}
void process_TRONs_msgs(){
/* note: TRONs communication after start is not guaranteed to be synchronous,
thus incoming messages must be checked for their content */
while (true){
// get 4 bytes at a time as an int32
byte bytes[4];
if (!read_4_bytes_nonblock(socket_fd, bytes))
break; // no bytes left to process
int32_t next = network_bytes_to_int_32(bytes);
// bytes are acknowledgement
if (next == ACK_SINGLE) {
if (--acks_missing < 0) throw "too many acknowledgements";
ROS_INFO("got acknowledgement for output");
continue;
}
// bytes are channel identifier
// find corresponding channel
const Channel *chan = nullptr;
for (Mapping& map : mappings) if (map.channel.identifier == next) chan = &map.channel;
/* note: this only happens if message number 12 in TRON User Manual is received,
which should not be the case according to the documentation */
if (chan == nullptr)
throw "channel could not be identified";
ROS_INFO("got channel identifier (%s) for input", chan->name.c_str());
// channel identified, assuming all following bytes are correct
// get number of variables
recv(socket_fd, bytes, 2, MSG_DONTWAIT);
byte_info(bytes, 2, false);
uint16_t var_count = network_bytes_to_uint_16(bytes);
ROS_INFO("got variable count %i", var_count);
int32_t vals[var_count];
// process variables
for (uint16_t i = 0; i < var_count; i++) {
recv(socket_fd, bytes, 4, MSG_DONTWAIT);
next = network_bytes_to_int_32(bytes);
std::string var = chan->vars[i];
ROS_INFO("got variable number %i: value of %s is %i", i+1, var.c_str(), next);
vals[i] = next;
}
for (Mapping& map : mappings)
if (map.channel.name == chan->name && map.channel.is_input)
for (ros::Publisher& pub : input_publishers){
if (pub.getTopic() == map.topic) {
if (map.input_callback != nullptr)
map.input_callback(map, vals);
else throw "no callback declared";
break;
}
}
// send acknowledgement
add_int32_in_network_order(ACK_SINGLE, bytes, 0);
ROS_INFO("sending acknowledgement");
send_bytes(socket_fd, bytes, 4);
}
}
int main(int argc, char**argv){
ros::init(argc, argv, "TRON dapter");
ros::NodeHandle nh;
try {
socket_fd = create_connected_socket(IP, PORT);
configuration_phase(nh);
request_start();
// testing phase loop
ros::Rate test_phase_freq(60);
while (ros::ok()) {
ros::spinOnce();
process_TRONs_msgs();
test_phase_freq.sleep();
}
} catch (const char* err){
ROS_FATAL("shutting down: %s", err);
ros::shutdown();
} catch (...){
ROS_FATAL("shutting down");
ros::shutdown();
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment