Ask Your Question
0

actionlib support in roscs?

asked 2012-07-10 22:58:31 -0600

Alireza gravatar image

Hi, Does ROSCS supports actionlib and also using custom messages?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2013-12-15 20:16:16 -0600

tfoote gravatar image

Reading the documentation at http://wiki.ros.org/roscs appears to support custom messages but there does not appear to be actionlib support.

edit flag offensive delete link more
1

answered 2013-12-15 23:51:06 -0600

Tully is right. You need to send the msgs, which are autogenerated by actionlib and then you are missing the support of actionlib server and/or client. Here is one messy little class that did the job for us so far.

using System;
using System.Threading;
using System.Collections;
using System.Collections.Generic;
using RosCS;
using RosCS.geometry_msgs;
using RosCS.nav_msgs;
using RosCS.tmc200_msgs;
using RosCS.actionlib_msgs;
using RosCS.move_base_msgs;
using RosCS.Services;
using RosCS.Services.nav_msgs;
using Castor;
public class ArmHelper
{
    public static Pose PointOfInterest { get; private set; }
    public static bool NewPOI { get; set; }
    public static bool ExplorationDone { get; set; }
    public static GoalID CurrentGoalID{get; private set;}
    public static GoalStatusArray GoalStatus {get; private set;}
    public static MoveBaseActionFeedback GoalFeedback {get; private set;}
    public static MoveBaseActionResult GoalResult {get; set;}
    public static bool MotionEnabled { get; set; }
    public static string MotionType { get; set; }

    protected static Node rosNode;
    protected static int id = 0;
    protected static MoveBaseActionGoal MoveBaseGoal{get; private set;}
    protected static SystemConfig sc = SystemConfig.LocalInstance;
    protected static ArmHelper instance = null;
    protected static object lockobject = new object();

    public static ArmHelper Get() {
        if(instance == null) {
            lock(lockobject) {
                if(instance == null) {
                    instance = new ArmHelper();
                    //Thread.MemoryBarrier();
                }
            }
        }
        return instance;
    }

    protected static Publisher motionPub;
    protected static Publisher goalPositionPub;
    protected static Publisher mBaseGoalPositionPub;
    protected static Publisher mBaseCancelGoalPub;

    protected static string GenerateUniqueId()
    {
        id++;
        return "goal_" + id;    
    }
    protected static GoalID GenerateActionLibGoalId()
    {
        CurrentGoalID = new GoalID();
        CurrentGoalID.Stamp = RosSharp.Now();
        CurrentGoalID.Id = GenerateUniqueId();
        return CurrentGoalID;   
    }

    public static void RosInit()
    {           
        if(rosNode!= null) return;          
        rosNode = Node.MainNode;            
        ExplorationDone = false;
        MotionEnabled = false;
        MotionType = sc["ImperaHelper"].GetString("ImperaHelper.PathHelper.MotionType");

        mBaseCancelGoalPub = new Publisher(Node.MainNode, "/move_base/cancel", GoalID.TypeId, 1);
        goalPositionPub = new Publisher(Node.MainNode, "/move_base_simple/goal", PoseStamped.TypeId, 1);
        mBaseGoalPositionPub = new Publisher(Node.MainNode, "/move_base/goal", MoveBaseActionGoal.TypeId, 1);
        motionPub = new Publisher(Node.MainNode,"MotionControl",MotionControl.TypeId,1);

        Node.MainNode.Subscribe("/move_base/status", OnGoalStatus);
        Node.MainNode.Subscribe("/move_base/feedback", OnGoalFeedback);
        Node.MainNode.Subscribe("/move_base/result", OnGoalResult);
        Node.MainNode.Subscribe("/PointOfInterest", OnPOI);
        Node.MainNode.Subscribe("/cmd_vel", OnTwist);           
    }

    protected ArmHelper()
    {       
        if(rosNode == null) {               
            RosInit();
        }
    }

    protected static void OnTwist(Twist tc) 
    {
        if (!MotionEnabled)
            return;         

        if(MotionType.Equals("tmc"))
        {
            MotionControl mc = new MotionControl();                                         
            mc.Motion.Angle = Math.Atan2(tc.Linear.Y, tc.Linear.X);
            mc.Motion.Translation = 1000*Math.Sqrt(tc.Linear.X * tc.Linear.X + tc.Linear.Y * tc.Linear.Y);
            mc.Motion.Rotation = tc.Angular.Z;  
            //Console.WriteLine("Velocity : " + mc.Motion.Translation + " Turn : " + mc.Motion.Rotation + " Angle : " + mc.Motion.Angle);
            motionPub.Send(mc);         
        }
    }   

    protected static void OnPOI(PoseStamped ps) {

        NewPOI = true;          
        PointOfInterest = ps.Pose;
        Console.WriteLine("PH: Got new POI");
    }

    protected static void OnGoalStatus(GoalStatusArray gStatus)
    {
        GoalStatus = new GoalStatusArray();
        GoalStatus.Header = gStatus.Header;
        GoalStatus.Status_list = gStatus.Status_list;
        //Console.WriteLine("Total Goal Count: "+GoalStatus.Status_list.Count);
    }

    protected static void OnGoalFeedback(MoveBaseActionFeedback feedback)
    {           
        GoalFeedback = new MoveBaseActionFeedback();
        GoalFeedback.Feedback = feedback.Feedback;
        GoalFeedback.Header = feedback.Header;
        GoalFeedback.Status = feedback.Status;
        //Console.WriteLine("Feedback: GoalID: "+GoalFeedback.Status.Goal_id.Id+ " Status: "+GoalFeedback.Status.Status);
    }

    protected static void OnGoalResult(MoveBaseActionResult result)
    {           
        GoalResult = new MoveBaseActionResult();
        GoalResult.Header = result.Header;          
        GoalResult.Result = result.Result;
        GoalResult.Status = result.Status;
        Console.WriteLine("Result: GoalID: "+GoalResult.Status.Goal_id.Id ...
(more)
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2012-07-10 22:58:31 -0600

Seen: 178 times

Last updated: Dec 15 '13